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Hypervelocity  Orbital  Intercept  Guidance 

Thesis  directed  by  Associate  Professor  Charles  E.  Fosha,  Jr. 


Terminal  guidance  of  a  hypervelocity  exo- atmospheric 
orbital  interceptor  with  free  end-time  is  examined.  The  pursuer 
is  constrained  to  lateral  thrusting  with  the  evader  modeled  as  an 
ICBM  in  its  final  boost  phase.  Proportional  navigation,  optimal 
control  using  certainty  equivalence,  dual  control,  and  control 
with  optimum  thrust  spacing  are  all  examined.  Also,  a  new 
approach  called  certainty  control  is  developed  for  this  problem. 

This  algorithm  constrains  the  final  state  to  a  function  of 
projected  estimate  error  to  reduce  control  energy  expenditure. 

All  methods  model  the  trajectories  using  splines  and  employ  eight 
state  Extended  Kalman  Filters  with  line-of- sight  and  range 

updates.  The  relative  effect  v-^ss  of  these  control  strategies 

f  f  f 

is  illustrated  by  applying  them  to  various  intercept  problems.  -  S 
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CHAPTER  I 

INTRODUCTION 

Orbital  interceptor  performance  can  be  enhanced  by  using 
a  terminal  guidance  law  that  incorporates  the  orbital  dynamics  of 
the  pursuer  and  evader  plus  the  error  knowledge  of  their 
estimates.  The  purpose  of  this  research  is  to  develop  a  guidance 
scheme  for  a  hypervelocity,  exo- atmospheric  orbital  vehicle  in  the 
final  thirty  seconds  of  flight  that  minimizes  lateral  thrusting 
while  attempting  to  intercept  a  boosting  missile.  Much  work  has 
been  done  on  the  most  common  form  of  intercept  guidance, 
proportional  navigation,  and  its  variations.  This  type  of 
navigation  assumes  that  the  force  of  gravity  acts  equally  on  the 
pursuer  and  evader  and  can  therefore  be  ignored  in  the  relative 
dynamics.  For  orbital  intercepts  with  large  initial  ranges  the 
force  of  gravity  will  affect  the  relative  trajectory  and  should  be 
included  in  the  equations  of  motion.  To  date,  analytic  solutions 
for  such  intercepts  exist  only  when  the  pursuer’s  impact 
conditions  are  prespecified. 

The  general  guidance  schemes  studied  in  this  research 
attempt  to  minimize  lateral  velocity  changes  by  varying  the  impact 
conditions  through  the  use  of  splines.  The  pursuer  is  modeled  as 


.  m  >.  ■  n  « Itj J  v.  ,'im  rj  -j  nj  mmjmtnjiAj  rjt  \u  mi  gj|  r.  B  ii.jH  V 


a  satellite  with  lateral  thrusting  capability  using  two- body 
orbital  dynamics.  The  evader  is  modeled  as  an  Intercontinental 
Ballistic  Missile  (ICBM)  in  its  final  boost  phase,  prior  to 
burnout.  The  relative  trajectory  is  propagated  numerically  to 
predicted  impact  time  and  then  approximated  by  splines, 
eliminating  the  need  to  repeatedly  propagate  new  trajectories  when 
present  conditions  are  varied.  A  search  is  then  made  for  a  new 
impact  time  and  point  that  will  minimize  present  interceptor 
velocity  changes  and  final  miss  distance. 

Six  different  variations  of  the  general  scheme  are 
derived.  The  first  scheme,  presented  in  Chapter  VI,  uses  a 
variable  weighting  factor  and  the  principle  of  certainty 
equivalence  to  reduce  velocity  changes  at  the  expense  of  final 
accuracy.  The  second  scheme,  also  presented  in  Chapter  VI,  is  a 
specialized  version  of  the  first,  determining  the  velocity  changes 
for  zero  miss.  The  third  guidance  algorithm  in  Chapter  VI  ignores 
the  effects  of  gravity  on  the  relative  trajectory  while  attempting 
a  zero  miss  solution.  The  fourth  scheme,  presented  in  Chapter 

VII,  optimizes  thrust  spacing  for  a  zero  miss  solution.  The  fifth 
scheme  employs  dual  control  techniques  to  reduce  estimation  error 
and  is  also  presented  in  Chapter  VII.  The  last  algorithm  is  r>  ne*-- 
control  approach  that  constrains  the  predicted  miss  distance  to  a 
function  of  final  estimator  error  and  is  presented  in  Chapter 

VIII.  Chapter  IX  summarizes  the  control  strategies. 
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Target  tracking  is  accomplished  with  a  ranging  device  and 
line- of- sight  sensors  for  in- plane  and  out- of- plane  measurements. 
Noise  corrupted  data  is  processed  through  an  eight  state  extended 
Kalman  Filter  with  serial  updates  occurring  every  tenth  of  a 
second.  The  Kalman  Filter  equations  are  contained  in  Chapter  X. 
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CHAPTER  II 

REVIEW  OF  LITERATURE 

Much  work  has  been  done  in  the  area  of  air-to-air 
xxitercept  guidance.  Guelman  has  derived  a  closed  form  solution 
for  pure  proportional  navigation  [l] ,  [2]  which  is  implemented  in 
Chapter  VI.  Peturbation  methods  have  been  employed  by  Sridhar  and 
Gupta  [3] .  Design  procedures  using  optimal  and  stochastic  control 
techniques  abound  [4]- [14]  with  variations  of  these  techniques 
implemented  in  Chapters  VI  and  VII.  In  the  works  cited  above,  the 
force  of  gra*-itj  is  assumed  to  act  equally  on  the  pursuer  and 
evader  and  is  ignored  in  the  relative  dynamics.  This  'flat  earth' 
assumption  is  adequate  for  air-to-air  encounters,  but  not  for 
space- to- space .  For  orbital  inte.cepts  with  large  initial  ranges 
the  force  of  gravit j  will  affect  the  relative  trajectory  and 
should  be  included  in  che  equations  of  motion. 

The  liteiuture  for  space- to- space  guidance  reveals  many 
numerical  approaches  for  determining  present  velocity  for  future 
rendezvous  [15] - [2r] .  date,  analytic  solutions  for  such 

intercepts  exist  only  when  the  pursuer’s  impact  conditions  are 
pre- specified  [19].  These  works  do  not  address  hvpervelocity 
intercept  involving  seconds,  but  are  concerned  with  a  much  slower 
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rendezvous  process  involving  hours  or  even  days.  Also,  most  of 
the  literature  reviewed  assumed  a  passive,  non- thrusting  target. 
The  literature  that  addressed  thrusting  targets  was  concerned  with 
evasive  maneuvering  or  ’gaining5,  the  most  recent  being  the  paper 
by  Menon  and  Calise  [21] .  A  Defense  Technology  Information  Center 
literature  search  revealed  that  the  few  papers  addressing  this 
problem  are  classified  and  therefore  unavailable  to  the  public. 

The  guidance  schemes  presented  here  attempt  to  minimize 
lateral  velocity  changes  by  varying  the  impact  conditions  through 
the  use  of  splines.  Splines  were  used  by  Johnson  [16]  in 
presenting  a  possible  Earth- Mars  transfer  guidance  algorithm. 
Dickmanns  and  Veils  nave  used  third  order  polynomials  for  general 
trajectory  optimization  [22],  as  well  as  Hargraves  and  Paris  [23]. 
The  splines  eliminate  the  need  to  repeatedly  propagate  new 
trajectories  when  conditions  are  varied,  resulting  in  faster 
searches.  This  feature  makes  them  attractive  for  a  hypervelocity 
orbital  intercept  where  a  fast  and  reasonably  accurate  numerical 
search  is  needed.  Spline  approximations  are  presented  in  Chapter 
V. 
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CHAPTER  III 

SYSTEM  MODELING 

In  this  chapter,  the  equations  of  motion  for  the  evader 
and  pursuer  are  developed,  along  with  the  necessary  coordinate 
transformation  for  pursuer  thrusting.  Atmospheric  drag  will  not 
be  considered  in  the  dynamics  because  the  interceptor  is  assumed 
exo- atmospheric.  Also,  due  to  the  pursuer’s  lateral  thrusting 
limitation,  the  longitudinal  axis  will  be  assumed  parallel  to  the 
pursuer’s  initial  velocity  vector. 

It  is  convenient  to  transform  the  present  coordinate 
frame  to  align  the  x  axis  with  the  pursuer’s  initial  velocity 
vector.  This  is  done  by  first  rotating  about  the  y  axis  until  the 
z  component  of  velocity  is  eliminated, 

y 


Figure  3-1.  Rotation  of  coordinate  frame  about  the  v  axis. 
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resulting  in  the  following  orthogonal  transformation  matrix: 


*  2  •  2 
X  +  Z 


o 


0  1  0 


-I 


0 


(3-1) 


/ 

The  second  rotation  is  about  the  new  z  axis  (z  ), 
eliminating  the  y  component  of  velocity. 


r 


Figure  3-2.  Rotation  of  the  coordinate  frame  about  the  t  axis. 
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This  rotation  yields  the  transformation  matrix: 


*  2 
+  zf 


(3-2) 


0 


0 


0 


0  1 


(3-3) 


Multiplying  the  two  matrices  in  the  proper  order  produces 
the  overall  transformation  matrix: 


P]  -  Pj]  PJ  (3-4) 

The  pursuer  is  modeled  as  a  satellite  traveling  in  excess 
of  twelve  kilometers  per  second  with  lateral  thrusting  capability 
using  two- body  orbital  dynamics.  Thrusting  is  prohibited  along 
the  longitudinal  (x)  axis  to  prevent  sensor  contamination  and  to 
satisfy  the  structural  constraints  of  having  forward  sensors  and  a 
large  aft  booster  to  achieve  hypervelocity  speed.  The  equations 
of  motion  are: 
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-/i  xr 


XP  = 


(x2  +  y2  +  z 
v  P  P  P 


(3-5) 


m  yT 


V 


+  a.. 


(x2  +  y2  +  z*) 
v  P  Jp  P; 


3/2 


(3-6) 


-H  z 


2P  ' 


+  a. 


(x2  +  y2  +  z2)^2 
P  P  P 


(3-7) 


where  ay  and  az  are  the  lateral  thrust  accelerations,  yx  is  the 
earth’s  gravitational  constant,  and  the  double  dots  denote  the 
second  derivative  with  respect  to  time. 

The  evader  is  modeled  as  an  Intercontinental  Ballistic 
Missile  (ICBM)  in  its  final  boost  phase  using  two-bf'dy  orbital 
dynamics.  For  tracking  purposes  the  intercept  must  occur  prior  to 
burnout.  Acceleration  due  to  thrusting  is  computed  in  the 
direction  of  the  booster’s  velocity  vector.  The  equations  of 
motion  are: 


A  = 


A 


o 


i  -  »0t 


(3-8) 


10 


^  XE 


A  x 


E 


X-n  - 


E'  (4  +  444)3/2  +  (4  +  444>1/2 


(3-9) 


^  ?e 


A  y 


yE 


y£ 


(4  *  4  *  4)3/2  +  (4  *  4  *  4) 1/2 


(3-10) 


’/*  Zt 


A  z 


E 


Zt>  - 


B  (4 4  4  *  4) 3/2  <4 4  4 4  4> 1/2 


(3-H) 


where  A  is  the  present  acceleration,  Aq  the  initial  acceration,  mQ 
the  initial  mass  flow  rate  divided  by  mass,  and  t  the  time  since 
ignition.  The  single  dot  denotes  the  first  derivative  with 
respect  to  time. 
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CHAPTER  IV 

PROBLEM  STATEMENT  AND  TRUTH  MODEL 


Time- to- go  and  pursuer  velocity  changes  are  the  control 
parameters  that  must  be  varied  to  minimize  miss  distance  and  fuel 
expended  (i.e,  velocity  changes).  This  can  be  done  by 
establishing  a  time  remaining  until  intercept  (time- to- go) , 
propagating  the  equations  of  motion  forward,  and  determining  the 
miss  distance.  An  iterative  process  can  then  be  used  to  find  the 
pursuer  velocity  needed  to  bring  the  miss  distance  to  zero.  The 
difference  between  current  velocity  and  that  needed  for  intercept, 
known  as  velocity- to- go,  must  be  minimized.  To  accomplish  this, 
the  time- to- go  is  varied  and  the  above  procedure  repeated  until  a 
minimum  velocity- to- go  is  found. 

The  computation  of  needed  velocity  is  time  consuming 
because  the  equations  of  motion  are  nonlinear  and  do  not  lend 
themselves  to  closed  form  solution.  These  equations  must  be 
propagated  numerically  to  intercept  time  whenever  the  initial 
velocity  is  varied.  The  above  method  will  serve  as  the  basis 
(truth)  model  for  this  control  problem  using  the  numerical 
techniques  found  in  Maron  [24] . 

A  Newton-  Raphson  method  for  solving  nonlinear  systems  is 
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employed  to  determine  the  proper  values  of  the  control  parameters. 


Let 


J 


(4- 1) 


be  a  solution  of  the  nonlinear  system 


Y 

1 


*l(p) 

xE(tgo)  '  XP(V) 

0 

£2C“) 

= 

yE^go^  ‘  yp^go^  ’  AVgo 

= 

0 

f3(?) 

zE^go^  '  W  ‘  AVzV 

0 

where  t  is  the  time- to- go  and  the  pursuer’s  velocity  changes';  are 
AVy  and  AVz«  The  effect  of  small  velocity  changes  in  (4-2)  can  be 
considered  linear  because  the  pursuer  is  assumed  to  travel  at 
hypervelocity,  resulting  in  a  near  straight-line  trajectory.  Any 
error  caused  by  this  assumption  will  be  accounted  for  in  the 
succeeding  iteration  when  the  proposed  velocity  change  is 
incorporated  in  the  nonlinear  dynamics. 

The  initial  control  values  must  be  incrementally  changed 
to  satisfy  (4-2).  A  linear  approximation  of  the  f  vector  ror 


ftnuitoJtUff  jtk  mt  iru  ivjw  irvrj  jtu  vmvuvuvuvuj 


13 


charges  in  u  will  yield  approximate  increments  of  the  control 
parameters. 


The  linearized  system  becomes 


dAVy 

fl(p) 

[J] 

dAYz 

=  - 

f2(-u) 

dt 

go 

i3(p) 

(4-3) 


where  J  is  the  Jacobian  matrix  of  the  f  vector  evaluated  at  u: 


[J]  = 


«!(?) 

&ix(n) 

«x(?) 

■w r 

go 

5f2(u) 

«2(») 

5f2(u) 

-w~ 

-OT7 

0V 

go 

«3(») 

«3(») 

«3(«) 

T£V 

■»*, 

"^r 

go 

(4-4) 


Computing  the  partial  derivatives  yields 
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[J]  = 


0  0  {%(tg0)  -  ip(tg0)} 


V  0  •  MV '  4V 


0  -t  {zpft  )-z(t  )  -  AY  ) 
go  *•  go'  pv  go'  zJ 


(4-5) 


To  determine  changes  in  the  u  vector,  f  is  multiplied  by  the 
negative  inverse  of  J 


dAVy 

fx(y) 

dAVz 

-  -  w* 

fj(j) 

dt„A 

go 

f3« 

(4-6) 


{MV  '  Vv  V^V1 


W  ■  W  -  tv«  0 
<MV  •  MV*1*® 


1 

<MV  •  VV> 


0 


0 


(4-7) 
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To  find  the  control  parameters  the  following  procedure 
should  be  used.  First,  establish  a  time- to- go  with  zero  velocity 
changes,  a  good  choice  being  the  time- to- go  that  yields  the  point 
of  closest  approach.  This  time- to- go  is  determined  by  propagating 
the  orbits  forward  until  a  minimum  relative  distance  is  reached. 
Because  the  evader  is  assumed  to  be  in  its  final  boost  phase 
throughout  the  intercept,  this  time- to- go  will  be  less  than  or 
equal  to  time  until  ICBM  thrust  termination.  Second,  propagate 
the  dynamic  equations  (3-5  thru  3-11)  forward  to  the  intercept 
time  and  determine  the  f  vector  from  (4-2).  Changes  to  the 
control  parameters  are  then  obtained  from  (4-6).  The  velocity 
changes  are  applied  to  the  pursuer’s  initial  conditions  and  the 
procedure  is  repeated  with  the  updated  time- to- go  until 
convergence  occurs.  The  resulting  control  parameters  will  drive 
the  miss  distance  to  zero  with  minimum  velocity  changes.  The 
difference  between  needed  and  present  velocity  are  sufficient  to 
determine  the  pursuer's  thrust  profile. 
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CHAPTER  V 

SPLINE  APPROXIMATIONS 

As  discussed  in  Chapter  IV,  numerical  propagation  of  the 
dynamic  equations  is  very  time  consuming.  It  would  be  convenient 
to  approximate  the  relative  trajectory  by  a  polynomial, 
eliminating  the  need  for  repeated  propagation.  Cubic  splines  lend 
themselves  well  tc  this  application  [16] ,  [22] ,  [23] .  The  current 
and  final  states  can  be  used  to  generate  cubic  splines  along  each 
axis  of  the  form 

*(0  =  it j!0  .  ♦  Ctg0  *  D  (5- 1) 

By  setting  the  current  time  to  zero,  D  and  C  become  the 
current  position  and  velocity  respectively,  with  time- to- go  being 
the  intercept  time.  Changes  in  velocity  will  be  reflected  only  in 
the  C  coefficient  and  the  final  state  can  be  easily  determined  for 
any  intercept  time.  Vith  this  formulation,  the  determination  of 
the  spline  coefficients  is  relatively  simple.  The  current  state 
gives  D  and  C  vith  no  computations: 

D  a  x (o)  (5-2) 
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C  =  x(o) 


(5-3) 


The  A  and  B  coefficients  can  be  computed  using  the  final 
states  and  (5-1)  as  follows: 


x(t  )  =  At)L  +  BtjV  +  Ct__  +  D 
v  go'  go  go  go 

(5-4) 

x(t  )  =  3AtJL  +  2Bt  +  C 
v  go'  go  go 

(5-5) 

Because  there  are  only  two  unknowns  in  the  above  two  equations, 
algebraic  manipulation  yields: 


A  = 


2[x(o)  -  x(tg0)]  [i(o)  -  x(tg0)] 


(5-6) 


go 


go 


B  = 


3[x(tg0)  -  x(o)]  [2x(o)+  x(t  )] 


(5-7) 


go 


g° 


Figures  depicting  distance  errors  associated  with  these 
approximations  are  provided  in  Appendix  A. 

There  is  an  added  versatility  in  using  splines.  Should 
the  system  model  be  changed,  only  the  spline  coefficients  need  be 
changed.  The  search  algorithms  based  on  the  splines  will  remain 
the  same,  operating  with  the  new  coefficients.  This  is  very 
beneficial  as  it  is  far  simpler  to  recompute  the  coefficients  than 


* 

I 


s 
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to  alter  the  algorithms. 

To  ensure  accuracy,  new  spline  coefficients  are  computed 
every  cycle  time.  To  accomplish  this,  the  truth  model  is 
propagated  forward  to  predicted  impact  time  to  obtain  the  needed 
final  states.  By  using  these  updated  final  states  every 
iteration,  propagated  roundoff  error  is  eliminated  in  the  spline 
coefficient  computations. 
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CHAPTER  VI 

OPTIMAL  CONTROL  FORMULATION  USING  CERTAINTY  EQUIVALENCE 

Changes  in  pursuer  lateral  velocity  will  affect  final 
position,  velocity  and  time.  These  effects  can  be  easily  computed 
with  the  relative  trajectory  modeled  by  splines  in  the  coordinate 
system  discussed  in  Chapter  II.  The  optimal  control  problem  is  to 
find  the  intercept  time  that  minimizes  changes  in  pursuer  velocity 
while  ensuring  a  hit.  Techniques  to  solve  such  problems  are 
addressed  by  Bryson  and  Ho  [25]  and  summarized  in  the  following 
paragraphs . 


PLAN  A 


To  solve  this  problem,  a  relative  spline  equation  is 
formed  for  each  axis  and  a  cost  function  is  established.  The  cost 
function  (L)  incorporates  velocity  changes  and  miss  distance 
multiplied  by  a  weighting  factor  (K)  and  is  represented  as 


L  = 


X,2) 


2 


2 


(6-1) 
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where  the  final  relative  state  vector  is  determined  from  the 
spline  equations: 


‘  * 

“  • 

‘ 

X1 

x(tgo) 

At  +  B  t  +  Ct  +  D 

X  go  X  go  X  go  X 

x2 

= 

y(t6o> 

- 

A  t3  +  B  t2  +  (C  -  AV  )t  +  D 
y  go  y  go  v  y  y;  go  y 

x3 

■<v 

A  t3  +  B  t2  +  (C  -  AV  )t  +  D 
z  go  z  go  K  z  V  go  z 

.  . 

(6-2) 


The  cost  function  must  now  be  minimized  with  respect  to 
the  control  vector  u: 


U1 

t 

go 

u  = 

u2 

= 

lvy 

u3 

»  « 

«. 

(6-3) 


As  stated  in  Bryson  and  llo  [25],  it  should  be  possible  to  find  a 
set  of  controls  such  that 


dl 


7X7  =  0 


(6-4) 


Three  equations  arise  from  (6-4)  with  three  unknowns,  expressed 
here  in  vector  form  as 


=  0  (6-5) 


with  x  being 


X. 

3A  tjL  +  2B  t  „  +  C 

1 

X  go  X  go  X 

X  = 

Xn 

_ 

3A  tj*  +  2B  t  +  C  -  AV 

2 

y  go  y  go  y  y 

X„ 

3A  t*  +  2B  t  +  C  -  AV 

3 

z  go  z  go  z  z 

\ 

K(x1xi  +  x2x2  +  x3x3) 

tu 

_ 

AV  -  Kx0t 

2 

y  2  go 

h0 

AV  -  Kx.t 

2 

,  z  3  go 

As  in  the  truth  model,  a  Newton- Raphson  method  from  Maron 
[24]  is  used  to  solve  (6-5).  It  is  important  to  note  that  this 
formulation  differs  from  the  truth  model  in  two  areas.  First,  a 
weighting  factor  has  been  introduced  that  allows  a  trade-off 
between  miss  distance  and  velocity  changes.  Zero  miss  distance  is 
associated  with  infinite  K,  while  zero  K  produces  no  velocity 
change.  Second,  the  splines  eliminate  the  need  for  repeated 
trajectory  propagation,  significantly  reducing  control  parameter 
search  time. 
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The  Jacobian  for  (6-5) 

is 

J11 

J12  J13 

[J]  = 

J21 

J22  0 

(6-7) 

J31 

0  J33 

where 

jn  =  j(=i[6yg0  *  2y +  *  2By]  * 


x3^ztgo  4  2BJ  +  *1  +  *2  +  x3^ 

(6-8) 

J12  =  J21  =  "K(x2  +  x2lgo^ 

(6-9) 

J13  J31  =  'K^x3  +  Vgo) 

(6- 10) 

J22  =  J33  =  1  4  Ktgo 

(6-11) 

Changes  in  the  control  vector  are  determined  by 


dt 

h. 

go 

1 

du  = 

dAVy 

“•[J]'1 

h0 

dAV 

z 

^  J 

.  h3  . 

(6-12) 
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J22 

'  J12 

"  J13 

[J]-1  = 

J12 

(Jll  "  *^13^22^ 

( J12J 13/ J22) 

~  J13 

(J12J13/J22^ 

(Jll  '  J12/J22^ 

(J11J22  "J13  ' 

J12J 

To  execute  this  procedure,  initialize  time- to- go 
(preferably  to  the  point  of  closest  approach)  and  determine  the 
spline  coefficients  for  this  initial  trajectory.  Compute  the  x, 
x,  and  h  vectors,  in  that  order.  Update  the  u  vector  using  (6-12) 
and  test  for  convergence.  If  convergence  is  not  achieved 
recompute  the  above  vectors  and  test  again. 

PUN  B 

The  formulation  in  this  plan  uses  splines  to  determine 
the  control  parameters  for  a  zero  miss  solution.  This  is  a 
specialized  version  of  Plan  A  where  the  weighting  factor  is  set  to 
infinity  (K=®).  Because  the  only  control  for  miss  in  the 
longitudinal  direction  is  time- to- go,  x1  in  (6-2)  is  set  equal  to 
zero, 


X1  =  Axlgo 


B  t  + 
x  go 


C  t 
X  go 


+  Dx  =  ° 


(6-14) 
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and  t  solved  using  numerical  techniques, 
established,  (6-2)  is  again  used  with  X2  and  x^ 
yielding  equations  for  the  velocity  changes: 


With  time- to- go 
equal  to  zero, 


4vy  =  Vg°  +  Bytgo  +  cy  4  W  (6'1S> 

4Vz  *  Vgo  *  Vgo  *  cz  +  W  <6-16) 


This  plan  is  computationally  less  burdensome  than  Plan  A  because 
the  complexity  of  the  search  is  reduced. 


PLAN  C 


Vithin  a  few  seconds  of  intercept  the  acceleration  due  to 
gravity  will  be  nearly  identical  for  the  pursuer  and  evader. 
Also,  the  booster,  still  thrusting  in  the  final  boost  phase,  will 
travel  in  a  near  straight  line  along  its  velocity  vector. 
Ignoring  gravity  terms  in  the  relative  dynamics  leads  to  a  simpler 
and  faster  solution,  reducing  the  guidance  to  proportional 
navigation  [1].  The  relative  trajectories  are  expressed  as 


("oV)1 

i(i-l) 


(6-17) 
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i(tg0)  =  { [xE(0)  -  Xp(0)]  +  [iE(0)  -  ip(0)]tgo 


1^(0) 

*j[(o)  +  yjj(o)  *  ig(o) 


(6-18) 


y(tg0)  =  { [yE(o)  -  yp(o)]  +  [yB(o)  -  yp(o)  -  ivy]tg0 


J>AyE(°)  (e-  is) 

+  ■  '  - - - 

ijs(°)  *  yE(°)  *  i|(0) 

l 


2  ■  zp(°)]  4  [V°>  '  ZP(0)  '  "J»go 

BAiE(0)  (6-20) 

+  zz . .  > 

i|(0)  *  yg(O)  *  i|(0) 

where  is  the  distance  associated  with  thruster  acceleration  in 
the  direction  of  booster  velocity.  As  in  Plan  B,  time- to- go  is 
computed  for  zero  miss  on  the  x  axis  using  (6-18),  and  then  the 
velocity  changes  can  be  found  from  (6-19)  and  (6-20). 
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CERTAINTY  EQUIVALENCE 

It  should  be  noted  that  all  these  techniques  use  the 
principle  of  certainty  equivalence,  where  expected  values  from  a 
state  estimator  are  substituted  for  random  variables  [26] .  The 
pursuer’s  states  are  assumed  known,  but  because  the  evader’s 
states  must  be  estimated,  the  resulting  system  is  stochastic. 
Optimal  control  formulation  is  based  on  a  system  that  is 
deterministic.  In  applying  the  certainty  equivalence  principle, 
the  stochastic  system  is  replaced  by  a  deterministic  one,  using 
the  expected  values  of  the  random  variables  from  the  estimator. 

There  is  a  drawback  to  this  technique  in  the  sense  that 
imperfect  knowledge  of  the  present  state  produces  needless 
thrusting.  Any  errors  in  the  present  state  estimate  cause  errors 
in  the  predicted  final  state.  This  results  in  the  computation  of 
velocity  changes  based  on  the  incorrect  final  state.  Future 
iterations  produce  similar  results  requiring  the  pursuer  to  thrust 
excessively. 

This  excessive  thrusting  can  be  reduced  using  stochastic 
control  techniques.  Three  formulations  are  examined  in  the 
followint  chapters.  The  first  determines  the  optimum  spacing  of 
corrective  thrusts  for  Plan  B.  The  second  uses  dual  control 
methods  based  on  predicted  error  knowledge,  such  as  filter 
covariance.  The  third  constrains  the  miss  distance  to  a  function 
of  predicted  error  knowledge,  at  the  expense  of  accuracy. 
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CHAPTER  VII 

STOCHASTIC  CONTROL 

OPTIMUM  SPACING  OP  CORRECTIVE  THRUSTS 

Corrective  thrusting  in  the  presence  of  state  estimate 
errors  can  be  optimally  spaced  to  reduce  fuel  [27] .  A  control 
effectiveness  ratio  ( p )  is  established  to  determine  the  spacing 
betwee.  thrusts.  This  ratio  directly  yields  thrust  times  when 
control  effectiveness  is  a  linear  function  of  time. 

For  this  formulation,  the  number  of  corrective  thrusts 
(N)  must  be  chosen  to  minimize  the  sum  of  thrusts  (S^,) ,  which  is 
total  AV.  The  behavior  of  AV  and  miss  distance  as  a  function  of  o 
can  be  produced  through  digital  computation  and  is  done  as  part  of 
the  simulation  to  determine  the  best  value  of  p  for  Plan  B. 

To  enhance  understanding  this  technique,  assume  the 
control  effectiveness  ratio  is  two  (p=2).  This  implies  that 
corrective  thrusting  should  take  place  when  the  control  has  half 
(1  fp)  the  effect  of  the  previous  corrective  thrust.  If  control 
effectiveness  is  a  near- linear  function  of  time,  as  is  the  case 
for  a  hypervelocity  vehicle,  then  it  will  be  halved  at  about  half 
the  time  to  impact  since  the  last  thrust.  Thrusting  will  take 


place  at  the  start  of  the  intercept,  at  one- half  time- to- go,  one 
fourth  time- to- go,  one-eigth  time- to- go  and  so  on.  Vith  p- 3  the 
optimum  thrust  timing  always  occurs  at  a  third  of  the  time- to- go 
since  the  last  correction.  Vhen  the  spacing  is  less  than  the 
estimator’s  cycle  time,  impact  is  imminent  and  thrust  is 
terminated. 

DUAL  CONTROL  FORMULATION 

Optimal  control  solutions  require  perfect  knowledge  of 
the  states,  but  in  reality  the  information  provided  to  the 
controller  is  only  an  estimate.  As  stated  by  Aoki  [28],  a  theory 
of  control  should  take  into  account  the  'imperfectness'  of 
information.  This  explains  the  need  to  incorporate  statistical 
decision  theory  in  control  formulation.  A  solution  that  uses 
imperfect  information  will  be  sub- optimal,  but  it  is  desirable  for 
such  a  solution  to  have  the  intrinsic  characteristics  of 
optimality  [5],  Recognition  that  the  control  affects  not  only  the 
state  but  also  its  uncertainty  leads  to  a  form  of  stochastic 
control  known  as  dual  control.  This  method  not  only  drives  the 
system  to  sGrae  final  state,  but  attempts  to  improve  state 
uncertainty  along  the  vay.  The  result  is  often  greater  accuracy 
and/or  reduced  fuel  consumption. 

A  dual  control  method  for  controlling  stochastic 
nonlinear  systems  with  free  end-time  was  developed  by  Tse  and 
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Bar- Shalom  [5].  This  method  differs  from  the  optimal  control 
formulation  presented  in  Chapter  VI.  Instead  of  minimizing  the 
cost  function  L  of  (6-1),  the  expected  value  of  the  cost  function 
(E{L})  is  minimized.  To  accomplish  this,  the  final  states  and 
their  covariances  must  be  computed.  This  can  be  done  by  running 
the  Extended  Kalman  Filter  forward  to  predicted  intercept  time,  as 
suggested  by  Tse,  Bar- Shalom  and  Meier  [4], 

The  solution  involves  establishing  an  expected  cost 
function  consisting  of  miss  distance  and  covariance  of  each  axis, 
along  with  the  control.  The  cost  function  L  from  (6-1)  is 
repeated  here  for  convenience: 

Of2  -  yf2  *  zf2)  (4V2  *  4^)  (7-1) 


Assuming  the  estimates  of  the  filter  are  Gaussian,  the  expected 
value  is: 


E{L}  =  K 


2  2  2  ‘2  ‘2  ‘2 

'xf  +  ?yf  +  iTzf  xf  4  >'f  +  zf 


AY2  +  AV2 
y  z 


2 


(7-2) 
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The  expected  cost  of  (7-2)  is  conditioned  on  the 
controls.  Two  cases  must  be  examined:  the  cost  associated  with 
the  certainty  equivalence  (CE)  solution  (Plan  A  of  Chapter  VI)  and 
the  cost  of  deviating  from  that  solution  to  improve  the  estimate. 
In  this  manner,  the  approximate  best  cost- to- go  includes  both 
estimation  and  control  performance. 

The  expected  cost  of  the  CE  solution  is  easily  computed 
by  determining  the  controls  from  Plan  A  and  then  running  the 
Extended  Kalman  Filter  forward  to  predicted  impact  time  assuming 
measurement  updates.  The  final  filter  data  is  then  inserted  into 
(7-2)  'to  find  the  expected  cost. 

Finding  the  expected  cost  of  deviating  from  the  CE 
solution  is  computationally  burdensome.  The  thrust  direction  that 
yields  the  greatest  estimate  improvement  must  first  be  determined. 
Thrusting  in  this  direction  will  cause  the  expected  miss  distance 
to  grow  due  to  departure  from  the  nominal  (CE)  path.  It  is 
therefore  necessary  to  determine  a  new  nominal  path  based  on  the 
deviation  and  include  the  control  energy  required  for  this  path  in 
the  deviation  cost  estimate.  Failure  to  do  so  may  result  in  large 
expected  miss  distances  that  erroneously  inflate  the  cost 
associated  with  deviation,  causing  the  CE  control  of  Plan  A  to 
a7 ways  be  chosen. 
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CHAPTER  VIII 

CERTAINTY  CONTROL 

As  stated  earlier,  if  the  estimate  is  near  perfect  then 
optimal  control  should  be  used.  For  a  less  accurate  estimate, 
dual  control  attempts  to  improve  the  measurement  certainty,  and 
thus  the  estimate,  by  expending  contro1  energy.  This  has  been 
shown  to  work  well  if  the  certainty  is  a  function  of  the  control 
parameters  [5].  Because  range  is  included  as  a  measurement, 
lateral  deviations  should  not  noticeably  improve  the  estimate. 
For  this  reason,  dual  control  techniques  are  not  expected  to  work 
better  than  certainty  equivalence  formulations. 

If  the  controls  associated  with  cost  do  not  affect  state 
estimate  certainty,  fuel  may  be  conserved  by  using  that  certainty 
to  reduce  the  controls.  By  linking  the  controls  to  the  certainty 
of  the  estimate,  a  near  perfect  estimate  would  yield  the  optimal 
control,  with  reduced  control  resulting  from  a  poor  estimate.  To 
accomplish  this,  the  predicted  final  states  are  constrained  by  a 
function  of  their  variances  at  the  final  time.  This  form  of 
control  will  be  called  certainty  control  and  is  implemented  by 
establishing  the  cost  function 


32 


AV2  +  AV2 

L  =  _y _ ! 

2 


(8-1) 


subject  to  the  constraint: 

2  2  2  -p  r  2  2  2  i 

f  =  Xf  +yf  *  2f  •  K["xf  *  V  +  ffzf]  <  0  (8-2) 

2 

where  K  is  a  weighting  factor..  The  final  state  estimates  (x^,  y^, 
z^)  and  their  deviations  (o^,  are  determined  by  running 

the  filter  forward  to  predicted  impact  time  without  updates  and 
then  representing  their  time  history  with  splines: 


X„  = 

A  t3  +  B  t2  4  C  t  4  D 

(8-3) 

s 

x  go  x  go  x  go  x 

y„  = 

it;*  4  b  t?A  4  c,t  4  n 

oo 

1 

Js 

y  go  y  go  y  go  y 

\  J 

= 

A  t3n  4  B  t2  C  t  +  D 

(8-5) 

s 

z  go  z  go  z  go  z 

xf  =  xs 

(8-6) 

■  f  "  '  S  ’  ^y^go 

(8-7) 

zf  =  zs  •  AVgo 

(8-8) 
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o  ,  =  A  t3  +  B  t2 
XI  ox  go  ox  go 

o  £  =  A  t3  +  B  t2 
yf  <ry  go  oy  go 

o  x  =  A  t3  +  B  t2 
zf  CZ  go  nz  go 


+  C  t 
ox  go 


+  C  t 
try  go 


+  C  t 
o%  go 


+  D 


ox 


(8-9) 

(8-10) 

(8-11) 


Conceptually,  the  constraint  produces  a  deviation  sphere 
about  the  predicted  impact  point.  If  the  predicted  miss  is  inside 
or  touching  the  sphere,  thrusting  is  not  necessary.  If  the 
predicted  miss  is  outside  the  sphere,  mininum  thrusting  is 
determined  to  bring  the  miss  to  the  surface  of  the  sphere.  As  the 
estimates  improve,  the  constraint  tightens  and  the  sphere  shrinks. 
The  spline  representations  allow  this  stochastic  problem  to  be 
solved  deterministically.  The  constraint  is  adjoined  to  the  cost 
function  to  form  the  Hamiltonian  [29] : 


H=  L  +  Xi 


(8-12) 


The  partials  of  H  with  respect  to  the  controls  must  equal  zero: 


r  -  lvy  '  Ayftgo  =  0 


(8-13) 


=  AV  -  Az,t  =  0 

SAT  z  f  6° 


(8-14) 


•STJ 


34 


=  MxfXf  +  yfyf  +  ZfZf  - 
K[‘rrfJxf +  VV  +  =  0  (8'15) 


with  the  dot  term  expansions  computed  in  Appendix  B. 

Equations  8-2,  8-13,  8-14,  and  8-15  constitute  four 
equations  with  four  unknowns,  which  can  be  reduced  two  equations 
and  two  unknowns  using  (8-7)  and  (8-8).  Substituting  (8-7)  into 
(8-13)  yields 


Vgo 


* 


(8-16) 


(8-17) 


In  a  similar  manner,  substituting  (8-8)  into  (8-14)  yields 


Azct 
s  go 


(6-18) 


(8- 19) 
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Equations  8-2  and  8-15  can  now  be  solved  in  terms  of  A 
and  t  .  Once  known,  AV^  and  AVz  can  be  determined  from  (8-16) 
and  (8-18).  The  parameters  A  and  t  can  be  found  by  numerical 
techniques  using  the  Jacobian: 


dt 

-f,j 

go 

- 

1 

dA 

,  . 

"f2 

(8- 20) 


= 


2  2  2 
Xf  4  yf  +  zf 


wr  2  2  2  I 

1 ["xf  +  "yf  * 


f2  =  Vf  +  Vi  *  Vf  - 


(8-21) 


Strxfhf  *  V" yf 4 


(8-22) 


with  the  elements  of  the  Jacobian  matrix  computed  in  Appendix  B. 

Should  t  .e  states  be  perfectly  known,  the  a  terms  will  be 
zero.  In  this  case,  the  equations  for  certainty  control  reduce  to 
the  op  .imal  control  formulation  for  Plan  B.  Should  the  estimate 
be  poor,  the  <r  terras  will  be  large  and  the  inequality  constraint 
of  vS-2)  is  met  with  very  little  (if  any)  change  in  velocity. 
This  demonstrates  the  principle  of  certainty  control,  where  the 
certainty  of  the  estimate  affects  control  energy  expenditure. 
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CHAPTER  IX 

SUMMARY  OF  CONTROL  STRATEGIES 

In  this  chapter  a  brief  summary  of  all  the  control 
strategies  is  presented.  It  is  intended  to  give  the  reader  a 
basis  for  quick  comparison.  The  cost  function  of  each  algorithm 
is  given,  along  with  the  requirements  for  computation. 

Plan  A  is  an  optimal  control,  certainty  equivalence 
formulation  that  minimizes  the  cost  function: 

t  _  K(x|  .  y|  ♦  z|)  4  (AVy  *  AV*) 

"  2  2 

This  algorithm  requires  an  estimate  of  the  final  relative  states. 


Plan  B  is  a  certainty  equivalence  formulation  that 
minimizes  the  cost  function: 
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subject  to  the  constraint 


f  =  Xj  +  =  0 

This  algorithm  requires  an  estimate  of  the  final  relative  states. 

Plan  C  is  a  certainty  equivalence  formulation  with  the 
same  cost  function  and  requirements  as  Plan  B.  The  difference 
between  these  strategies  is  that  gravity  is  ignored  in  the  dynamic 
equations  used  to  estimate  the  final  relative  states. 

The  optimal  spacing  of  corrective  thrusts  also  uses  the 
same  cost  function  and  requirements  as  Plan  B.  In  this  strategy, 
however,  the  pursuer  is  not  permitted  to  thrust  every  cycle  time. 
Thrust  timing  is  controlled  by  selecting  a  control  effectiveness 
ratio  to  minimize  control  energy  expenditure. 

Dual  Control  is  a  stochastic  control  formulation  that 
attempts  to  improve  the  estimate,  and  thus  accuracy,  by  minimizing 
the  cost  function: 
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E{1}  =  K 


2 


"2  "2  "2 
xf  +  yf  +  zf 


+ 


2 


+  AV2 
z 

2 


This  algorithm  requires  estimates  of  the  final  relative  states, 
their  filter  variances,  and  the  relationship  between  control  and 
variance. 


Certainty  Control  is  a  new  stochastic  control  formulation 
that  reduces  the  control  based  on  the  certainty  of  the  estimate  by 
minimizing  the  cost  function 


L  = 


2 


subject  to  the  constraint 


*2  ‘2  A2  p 
xf  +  yf  +  zf  '  1 


"xf  f  "yf 


2  ' 
+  "zf 


2 


<  0  . 


This  algorithm  requires  estimates  of  the  final  relative  states  and 
their  filter  variances. 


CHAPTER  I 


EXTENDED  SALMAN  FILTERING 


Optimal  estimates  of  the  pursuer  and  evader  are  needed 
for  the  search  algorithms  to  converge  properly.  Due  to  the  nature 
of  the  dynamics  and  sensors,  the  relative  position  and  velocity 
must  be  estimated  from  sampled  nonlinear  measurements.  The 
estimation  problem  for  a  nonlinear  system  having  continuous 
dynamics  and  discrete- time  measurements  is  addressed  by  Gelb  [30]. 
The  Extended  Kalman  Filter  (EKF)  was  chosen  over  other  estimation 
methods  because  the  optimal  estimate  is  determinate.  That  is,  the 
dynamics  and  observations  of  the  pursuer  and  evader  can  be  well 
predicted  in  the  presence  of  Gaussian  noise. 

A  summary  of  the  continuous- discrete  EKF  algorithm  from 
Gelb  [30]  follows.  The  equations  for  the  state  dynamics  and 
measurements,  as  well  as  the  computations  of  the  partial 
derivatives,  can  be  found  in  Appendix  C.  The  system  model  is  a 
continuous  model  of  the  state  dynamics  with  white  Gaussian  noise 
{w(t)}  added. 


x(t)  =  f (x(t) ,t)  +  w(t) 


(10-1) 
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w(t)  '  K(0,Q(t))  (10-2)  | 

I 

i 

where  w  is  a  Gaussian  (normal)  random  vector  with  mean  0  and 
covariance  matrix  (J.  The  measurement  model  is  discrete  and 

corrupted  by  white  Gaussian  noise  v,  :  j 

I 

j 

?k  =  .hk(?(tk))  +  -vk  (10‘3)  1 

vk  a  N(0,Rk)  .  (10-4) 

In  either  case,  the  noise  is  assumed  uncorrelated  for  all  t(k). 

The  state  estimate,  denoted  by  a  hat,  is  propagated  from 
(10-1)  with 


?  ■  f (y(t) ,t) 


(10-5) 


and  the  error  covariance  P(t)  is  propagated  by 


P(t)  =  F(x(t),t)P(t)  ♦  P(t)FA(x(t),t)  *  Q(t) 


^f(x(t),t) 

F(?(t)’t)  =  0x(t) 


(10-7) 


x(t)  =  x(t) 


The  measurements  determine  the  gain  matrix  through  the 

equations 


dhk(x(tk) 

=  &c(tk7 

where  the  (-)  symbolizes  prior  to  update  and  the  (+)  after  update. 

Vith  the  gain  matrix  computed,  the  state  estimate  and 
error  covariance  can  be  updated  by  the  following  equations: 


(10-8) 

?(tk)  =  ?k(-) 

)Kk(xk(-))  +  &k]* 1  (10-9) 


xk(+)  -  xk(-)  +  Kk[zk  -  hk(xk(-))]  (10-10) 

Pk(*)  =  [I-KkBk(xk(-))}Pt(  )  ■  (10'U) 

It  is  advantageous  to  process  measurements  one  at  a  time. 
This  method,  called  serial  updating  [31],  eliminates  the 
requirement  to  compute  a  matrix  inverse,  thereby  reducing  computer 
load  and  avoiding  the  computational  problems  associated  with 
inverting  an  ill-conditioned  matrix.  Also,  measurements  may  be 
skipped  without  reformulating  the  filter  equations,  allowing 
greater  flexibility  in  examining  various  tracking  schemes.  The 
simultaneous  measurement  components  of  the  vector  zk  can  be 
considered  serially  over  a  very  short  time  span. 
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CHAPTER  XI 

COMPUTER  SIMULATION 


A  menu- driven  program  that  simulates  all  the  algorithms 
was  written  in  FORTRAN  77  and  run  on  a  VAX  8600.  The  code  for 
this  program  can  be  found  in  Appendix  D.  Six  cases  are  examined 
to  determine  the  accuracy  and  efficiency  of  each  algorithm.  In 
all  cases  the  propagation  (w)  and  measurement  (v)  noise  properties 
associated  with  the  filter  are: 

2 

wv  v  ,(*)  “  2.21516  x  10' 18 

s 

V-  •  .(t)  *  N(0,  5.52049  x  10* 20  ^ ) 

A, J ,4  ^ 


2 

Vi ( t )  *  K(0,  4.29831  x  10* 12  m~) 

7 

v •  (t)  *  K(0,  2.493241  x  10*  7  J.) 

s3 

V^7(k)‘  N(0,  1.0  x  10'8) 


43 


VR(k)  *  N(0,  1.0  x  10' 8  x  R2  m2) 


where  0  is  the  out- of- plane  line- of- sight  angle,  7  the  in- plane 
line- of- sight  angle  and  R  is  range. 

The  startup  variances  are: 


2  2  2  2 

0  -  0  -  0  -  100  m 

xx  yy  zz 


2  2  2  ip 

xx  yy  zz  -z 

s 


2  _  .  nT 
"ii  -  -1  -4 

s 


<r?.  =  2.493241  x  10" 6  1 

irm  -7 


The  pursuer’s  initial  conditions  for  all  cases  are 

Xp  =  -359899.441  in 

x  =  11991.950  -® 

P  s 

y  =  6727335.870  m 
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yp  =  158.764 


m 

s 


z  =  0.0  m 

P 


zp  =  0.0 


m 

s 


o 

with  a  lateral  acceleration  range  of  3-60  m/s  in  each  axis 


The  booster’s  characteristics  are  modeled  as 


kn  =  3.15788 
o 


it! 


m  =  .01579 


with  time- to- go  equaling  30  seconds. 


A  time  lag  of  one  tenth  second  is  used  for  all  algorithms 
when  computing  velocity  changes.  It  is  unrealistic  to  assume  the 
filter  can  process  measurements,  the  controller  determine  thrust, 
commands,  and  the  thrusters  respond  to  those  commands  all 
instantaneously.  One  cycle  time  is  chosen  to  allow  the  velocity 
changes  computed  in  the  previous  cycle  to  be  implemented  in  the 
present  cycle.  The  controller  routines  are  built  to  take  this  lag 
into  account.  Also,  thrusting  is  not  permitted  during  the  first 
three  seconds  of  an  intercept  to  account  for  target  acquisition. 


The  following  page  shows  the  evader  initial  condition  for 
six  cases.  Case  I  represents  a  head  on,  in- plane  intercept.  Case 
II  represents  a  head  on,  10°  out- of- plane  intercept.  Case  III 
represents  a  head  on,  20°  out- of- plane  intercept.  Case  IV 
represents  an  in- plane  tail  chase.  Case  V  represents  a  10° 
out- of- plane  tail  chase.  Case  VI  represents  a  20°  out- of- plane 


tail  chase. 
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CHAPTER  XII 

RESULTS 


A  history  of  the  miss  and  velocity  changes  with  respect 
to  multipliers  is  generated  for  those  algorithms  requiring 
multipliers  (see  Figures  12-1  through  12-24).  A  specific 
multiplier  is  then  chosen  for  each  plan  for  inclusion  in  the 
tables  that  follow. 

One  hundred  Monte  Carlo  runs  are  generated  per  plan  per 
case.  The  mean  of  each  set  of  runs  is  adequate  for  judging 
relative  performance.  This  performance  is  recorded  in  the  six 
tables  that  follow  the  figures. 

Appendix  E  contains  the  in- plane  thrusl  profiles  for 
Cases  I  and  V  of  all  pirns.  In  the  appendix,  each  profile  uses 
the  same  random  seed  for  startup  to  show  the  effect  of  estimate 
uncertainty  on  the  various  control  strategies. 


AVERAGE  MISS  DISTANCE 


CONTROL  MULTIPLIER  (K) 


Figure  12-1.  Performance  of  Plan  A  for  Case  I. 


AVERAGE  TOTAL  AV  (METE! 


A  VCR  A 


y.A 


C  J 


DISTANCE 
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,  y 


AVERAGE  TOTAL  AV  (METERS/SECOND) 
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Table  12-1.  Case  I  Performance. 


(Head  On,  In-plane  Intercept) 


MEAN 

STANDARD 

MEAN 

STANDARD 

MISS 

DEVIATION 

AV 

DEVIATION 

(METERS) 

(METERS) 

(M/S) 

(M/S) 

PLAN  A 
(1*10) 

.465 

.230 

81.93 

5.64 

PLAN  B 

.362 

.173 

88.29 

7.00 

PLAN  C 

.362 

.174 

90.76 

6.86 

OPTIMUM 

THRUST 

SPACING 

(/>=1.75) 

.363 

.175 

36.63 

8.14 

DUAL 

CONTROL 

(**10) 

.465 

.230 

81.93 

5.64 

CERTAINTY 
CONTROL 
(K= .4) 

.399 

.215 

21.61 

3.79 

TRUTH 

WITH 

NOISE 

.527 

.264 

81.44 

6.76 

TRUTH 

VITHOUT 

NOISE 

0 

NA 

7.31 

NA 
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Table  12-2.  Case  II  Performance. 
(Head  On,  10°  Out- of- Plane  Intercept) 


MEAN 

STANDARD 

MEAN 

STANDARD 

MISS 

DEVIATION 

AV 

DEVIATION 

(METERS) 

(METERS) 

(M/S) 

(M/S) 

PLAN  A 
(K=10) 

.502 

.224 

83.82 

6.99 

PLAN  B 

.360 

.171 

90.39 

7.24 

PLAN  C 

.360 

.171 

93.07 

7.39 

OPTIMUM 

THRUST 

SPACING 

(/?=1.75) 

.361 

.171 

37.19 

8.50 

DUAL 

CONTROL 

(K=10) 

.502 

.224 

83.82 

6.99 

CERTAINTY 
CONTROL 
(K= • 4) 

.386 

.191 

23.21 

4.18 

TRUTH 

VITH 

NOISE 

.545 

.264 

83.69 

7.18 

TRUTH 

WITHOUT 

NOISE 

0 

NA 

7.54 

NA 
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Tabel  12-3.  Case  III  Performance. 


(Head  On,  20°  Out- of- Plane  Intercept) 


MEAN 

STANDARD 

MEAN 

STANDARD 

MISS 

DEVIATION 

AV 

DEVIATION 

(METERS) 

(METERS) 

(M/S) 

(M/S) 

PLAN  A 
(K=10) 

.506 

.225 

90.92 

7.51 

PLAN  B 

.358 

.168 

97.53 

7.93 

PLAN  C 

.358 

.168 

99.97 

7.89 

OPTIMUM 
THRUST 
SPACING 
(P= 1-75) 

.358 

.168 

39.87 

8.63 

DUAL 

CONTROL 

(K=10) 

.506 

.225 

90.92 

7.51 

CERTAINTY 

CONTROL 

(K=.4) 

.372 

.185 

24.45 

4.50 

TRUTH 

WITH 

NOISE 

.534 

.293 

90.77 

7.55 

TRUTH 

WITHOUT 

NOISE 

0 

NA 

7.78 

NA 
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Table  12-4.  Case  IV  Performance. 
(In- Plane  Tail  Chase) 


MEAN 

STANDARD 

MEAN 

STANDARD 

MISS 

DEVIATION 

AV 

DEVIATION 

(METERS) 

(METERS) 

(M/S) 

(M/S) 

PLAN  A 
(1=10) 

.204 

.105 

110.87 

9.89 

PLAN  B 

.126 

.058 

113.72 

9.81 

PLAN  C 

.126 

.057 

111.82 

9.25 

OPTIMUM 
THRUST 
SPACING 
(/>= 1.75) 

.124 

.057 

35.06 

10.34 

DUAL 

CONTROL 

(1=10) 

.204 

.105 

110.87 

9.89 

CERTAINTY 

CONTROL 

(K=.4) 

.150 

.081 

26.94 

6.78 

TRUTH 

VITH 

NOISE 

.376 

.221 

105.71 

8.93 

TRUTH 

WITHOUT 

NOISE 

0 

NA 

8.75 

NA 
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Table  12-5.  Case  V  Performance. 
(10°  Out- of- Plane  Tail  Chase) 


MEAN 

STANDARD 

MEAN 

STANDARD 

MISS 

DEVIATION 

AV 

DEVIATION 

(METERS) 

(METERS) 

(M/S) 

(M/S) 

PLAN  A 
(*=10) 

.190 

.100 

129.61 

13.29 

PLAN  B 

.126 

.061 

132.65 

13.32 

PLAN  C 

.126 

.061 

129.47 

12.26 

OPTIMUM 

TERUST 

SPACING 

(p=1.75) 

.126 

.059 

39.96 

12.85 

DUAL 

CONTROL 

(K=10) 

.190 

.100 

129.61 

13.29 

CERTAINTY 

CONTROL 

(K=.4) 

.136 

.076 

29.74 

9.10 

TRUTH 

VITH 

NOISE 

.379 

.204 

123.57 

11.61 

TRUTH 

WITHOUT 

NOISE 

0 

NA 

9.52 

NA 
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Table  12-6.  Case  YI  Performance. 
(20°  Out- of- Plane  Tail  Chase) 


MEAN 

STANDARD 

MEAN 

STANDARD 

MISS 

DEVIATION 

AV 

DEVIATION 

(METERS) 

(METERS) 

(M/S) 

(M/S) 

PLAN  A 
(*=10) 

.161 

.079 

160.42 

14.89 

PLAN  B 

.135 

.064 

162.73 

14.85 

PLAN  C 

.136 

.064 

157.91 

13.50 

OPTIMUM 
THRUST 
SPACING 
(p=l . 75) 

.135 

.065 

46.67 

15.90 

DUAL 

CONTROL 

(K=10) 

.161 

.079 

160.42 

14.89 

CERTAINTY 

CONTROL 

(K=*8) 

.171 

.087 

30.11 

10.40 

TRUTH 

VITH 

.396 

.233 

151.56 

13.37 

NOISE 

TRUTH 

WITHOUT 

NOISE 

0 

NA 

10.13 

NA 
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As  predicted,  the  dual  control’s  performance  is  no  better 
than  the  certainty  equivalence  formulation  of  Plan  A.  This  is  due 
to  the  fact  that  range  is  included  as  a  measurement,  causing  the 
control  to  have  virtually  no  effect  on  improving  filter  variance. 
Plan  B  is  more  accurate  than  Plan  A,  but  more  costly  in  energy. 
Again,  this  result  is  expected  because  the  formulation  of  Plan  B 
is  based  on  infinite  miss  penalty  (K=®)  for  Plan  A.  By  optimally 
spacing  the  thrusts  of  Plan  B,  energy  expenditure  is  considerably 
reduced  with  little  or  no  sacrifice  in  accuracy. 

Plan  C  is  just  as  accurate  as  Plan  B,  with  slightly 
greater  cost  resulting  from  large  initial  intercept  range.  This 
extra  cost  is  attributed  to  the  negligible  gravity  assumption  used 
in  the  formulation  of  Plan  C.  For  the  smaller  ranges  associated 
with  a  tail  chase,  Plan  C  was  actually  less  costly  than  Plan  B. 

In  every  case,  certainty  control  yields  the  least  energy 
expenditure.  This  result  is  not  surprising,  as  the  formulation  of 
certainty  control  is  based  on  reducing  control  energy  in  the 
presence  of  poor  estimates.  This  form  of  control  works  best 
because  filter  variance  is  range  dependent.  As  range  decreases, 
the  control  constraint  tightens,  and  accuracy  increases. 
Therefore,  less  fuel  is  used  when  range  is  great,  with  refinements 
made  as  impact  nears. 

The  last  two  entries  (truth  with  and  without  noise)  do 
not  use  splines.  Trajectory  changes  are  computed  as  outlined  in 
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Chapter  IV.  This  data  is  included  as  a  baseline  reference  of 
performance. 

Because  ranging  is  an  active  and  costly  process,  various 
tracking  schemes  were  examined  to  determine  if  ranging  is  needed. 
This  was  easily  done  in  the  simulation  because  of  the  serial 
updating  discussed  in  Chapter  X.  Using  only  line-of- sight 
measurement  angles,  all  algorithms  are  less  accurate  and/or 
require  more  velocity  changes.  It  is  of  interest  to  note  that  the 
dual  control  guidance  scheme,  true  to  its  nature,  did  expend 
control  energy  to  improve  the  estimate.  The  improvement  was  very 
slight  because  of  the  pursuer’s  speed  and  lateral  thrusting 
limits.  Allowing  one  range  update  at  midcourse  also  proved  costly 
for  all  guidance  schemes. 

An  attempt  was  made  to  reduce  the  order  of  the  filter  in 
the  hopes  of  reducing  processing  time.  The  result  was  a  serious 
degradation  of  performance  for  all  algorithms.  The  system  model 
is  very  sensitive  to  the  evader  booster  characteristics,  A  and  m, 
which  are  estimated  by  the  eight  state  filter.  Failure  of  the 
filtering  process  to  refine  initial  booster  estimates  allows 
greater  acceleration  errors  to  be  passed  on  to  the  evader 
estimates,  significantly  reducing  end-game  accuracy. 


80 


CHAPTER  XIII 

CONCLUSIONS  AND  AREAS  OF  FURTHER  RESEARCH 


In  this  research,  six  guidance  schemes  were  examined  to 
determine  their  capability  to  minimize  lateral  velocity  changes  of 
a  hypervelocity  orbital  intercept  vehicle.  Proportional 
navigation,  optimal  control  using  certainty  equivalance,  dual 
control,  control  with  optimum  thrust  spacing,  and  certainty 
control  were  all  examined.  Certainty  control  was  shown  to  be  the 
most  energy  efficient. 

Certainty  control  constrains  the  final  condition  to  a 
function  of  final  estimator  accuracy  in  the  absence  of  updates. 
This  general  approach  is  not  limited  to  hypervelocity  vehicles, 
and  would  suggest  other  applications  of  this  form  to 
stochastically  control  intercepts. 

T'iis  control  requires  a  measure  of  final  estimator 
accuracy  which  was  achieved  by  running  the  Extended  Kalman  Filter 
forward  to  intercept  time  without  updates.  This  time  consuming 
process  could  be  eliminated  if  filter  variances  could  be  estimated 
by  some  function  (polynomial  or  otherwise).  Also,  the  constraint 
multiplier  was  assumed  constant  for  this  formulation.  Perhaps  a 
multiplier  that  was  range  or  time  dependent  would  further  reduce 
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interceptor  thrusting. 

In  summary,  the  approach  identified  by  this  research  not 
only  improves  the  efficiency  of  hypervelocity  intercept,  but  can 
be  applied  to  a  broad  range  of  stochastic  problems  where  control 
energy  does  not  improve  filter  accurancy.  It  is  also  possible  to 
combine  the  effects  of  dual  and  certainty  control  in  certain  cases 
by  initially  using  dual  control  to  improve  estimator  accuracy  and 
then  switching  to  certainty  control.  End-game  accuracy  may  be 
improved  by  switching  from  certainty  control  to  a  certainty 
equivalence  formulation  just  prior  to  impact. 
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APPENDIX  A 

SPLINE  APPROXIMATION  ERRORS 

Because  the  splines  discussed  in  Chapter  V  a-e  only 
approximations,  there  will  be  a  small  difference  from  the  true 
trajectories  modeled  by  them.  To  examine  these  errors,  six 
figures  are  generated  from  a  worst  case  scenario..  Case  I  is 
considered  the  worst  because  of  the  high  relative  velocities. 

From  this  case  profiles  are  generated  for  no  velocity  change,  a 
velocity  change  of  one  meter  per  second,  maximum  AV  ,  and  maximum 

av  . 

z 

Figures  A- 1  and  A-  2  show  no  error  at  predicted  impact 

time.  This  is  expected  because  the  splines  are  constrained  to 
match  final  position  and  velocity.  Figure  A- 3  reflects  the  error 
caused  by  a  one  meter  per  second  in- plane  velocity  change 

decreasing  as  time- to- go  decreases.  Figure  A- 4  shows  a  similar 
effect  for  an  out- of- plane  velocity  change.  Figure  A- 5  shows  the 
effect  of  maximum  AY^  on  trajectory  error  in  the  region  of  impact. 
Figure  A- 6  shows  a  similar  effect  for  maximum  out- of- plane 

thrust ing. 


Figure  A-l.  Distance  error  of  spline  trajectory  vs.  time 
for  zero  velocity  change. 
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Figure  A- 3.  Distance  error  of  spline  trajectory  vs.  time 
for  A\‘v  =  i  m/s. 
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Figure  A-4.  Distance  error  of  spline  trajectory  vs.  time 
for  AV2  =  l  o/s. 
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Figure  A- 5.  Distance  error  of  spline  trajectory  vs.  time 

for  maximum  AV  (AV  =  6  m/s). 
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Figure  A- 6.  Distance  error  of  spline  trajectory  vs.  time 
for  maximum  AV„  (AV  =  6  m/s). 
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APPENDIX  B 

DERIVATION  OF  CERTAINTY  CONTROL  EQUATIONS 


The  dot  terms  for  (8-15)  are  computed  as  follows: 
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The  Jacobian  matrix  elements  for  (8-20)  are: 
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APPENDIX  C 

EXTENDED  KALMAN  FILTER  EQUATIONS 
The  EKF  states  are  defined  from  (3-5)  through  (3-11)  as  follows: 
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Determining  the  F  matrix  components  from  (10-7)  yields 
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Fgg  =  2m  .  (C- 29) 

All  other  elements  are  zero. 

The  measurements  of  range  and  line- of  sight  angles  are: 
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(C- 30) 
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The  vectors  for  serial  update  from  (10-8)  are: 
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APPENDIX  D 


COMPUTER  SIMULATION  PROGRAM 

Contained  here  are  the  routines  used  to  simulate  the 
guidance  algorithms  of  Chapters  IV  through  IX.  The  program  (main 
code;  is  separate  from  the  supporting  routines  (subroutines)  with 
the  following  labeling: 


1.  KEVSIM  -  This  is  the  main  program  for  the  hyper¬ 
velocity  orbital  intercept. 

2.  TOOL  1  -  Cortained  here  are  the  subroutines  needed 
for  orbit  propagation. 

3.  TOOL  2  -  The  -ooMinate  transformation  matrix  sub¬ 
routines  are  found  here. 

4.  TOOL  3  -  The  Extended  Kalman  Filter  subroutines  are 
kept  hei e . 

5.  TOOL  4  -  The  subroutines  for  all  the  guidance  algor¬ 
ithms  plus  the  truth  model  are  here. 


The  source  code  for  the  above  can  be  found  on  the 
following  pages.  All  the  code  ic  written  in  FORTRAN  77. 


mono  n  n  n 
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C  PROGRAM  KEWSIM 

C  THIS  PROGRAM  IS  A  HVPERVELOCITY  ORBITAL  INTERCEPT 
C  SIMULATION  THAT  FINDS  THE  VELOCITY  CHANGES  WITH  AN 
C  EIGHT,  SIX  OR  THREE  STATE  EXTENDED  KALMAN  FILTER 
C  AND  THREE  MEASUREMENTS. 

LINK  AS  FOLOWS: 

LINK  KEWSIM, TOOLl , TOOL2 , TOOL3 , TOOL4 
PROGRAM  DICTIONARY 
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AD 

ASIGX 
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AT 

AX 

AY 
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BX 

BY 

BZ 
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COV 

CVD 

CVDD 

CVDUAL 
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cx 

CY 

CZ 

DDX 

DDY 

DDZ 

DELTAY 

DELTAZ 

DH 

DSIGX 

DSIGY 

DSIGZ 

DTGO 

DUM 

DV 

DX 

DY 

DZ 

FILTER 


EVADER  ACCELERATION  DUE  TO  THRUSTING 
DUMMY  ACCELERATION 

SIGMA  X  AXIS  SPLINE  COEFFICIENT  OF  T**3 
SIGMA  Y  AXIS  SPLINE  COEFFICIENT  OF  T**3 
SIGMA  Z  AXIS  SPLINE  COEFFICIENT  OF  T**3 
DUMMY  ACCELERATION 
X  AXIS  SPLINE  COEFFICIENT  OF  T**3 

Y  AXIS  SPLINE  COEFFICIENT  OF  T**3 
Z  AXIS  SPLINE  COEFFICIENT  OF  T**3 
SIGMA  X  AXIS  SPLINE  COEFFICIENT  OF  T**2 
SIGMA  Y  AXIS  SPLINE  COEFFICIENT  OF  T**2 
SIGMA  Z  AXIS  SPLINE  COEFFICIENT  OF  T**2 
X  AXIS  SPLINE  COEFFICIENT  OF  T**2 

Y  AXIS  SPLINE  COEFFICIENT  OF  T**2 
Z  AXIS  SPLINE  COEFFICIENT  OF  T**2 
ITERATION  FINAL  COUNT 
COVARIANCE  MATRIX 

COVARIANCE  MATRIX  TRACE  ELEMENTS 

DUMMY  COVARIANCE  MATRIX 

DUMMY  COVARIANCE  MATRIX 

SIGMA  X  AXIS  SPLINE  COEFFICIENT  OF  T 

SIGMA  Y  AXIS  SPLINE  COEFFICIENT  OF  T 

SIGMA  Z  AXIS  SPLINE  COEFFICIENT  OF  T 

X  AXIS  SPLINE  COEFFICIENT  OF  T 

Y  AXIS  SPLINE  COEFFICIENT  OF  T 
Z  AXIS  SPLINE  COEFFICIENT  OF  T 
CHANGE  IN  X  VELOCITY  (INERTIAL  FRAME) 
CHANGE  IN  Y  VELOCITY  (INERTIAL  FRAME) 
CHANGE  IN  Z  VELOCITY  (INERTIAL  FRAME) 
CHANGE  IN  Y  VELOCITY  (BODY  FRAME) 

CHANGE  IN  Z  VELOCITY  (BODY  FRAME) 

DUMMY  STEP  SIZE 

SIGMA  X  AXIS  SPLINE  COEFFICIENT 

SIGMA  Y  AXIS  SPLINE  COEFFICIENT 

SIGMA  Z  AXIS  SPLINE  COEFFICIENT 

DUMMY  TIME-TO-GO 
DUMMY  VARIABLE 
INCREMENTAL  VELOCITY  CHANGE 
X  AXIS  SPLINE  COEFFICIENT 

Y  AXIS  SPLINE  COEFFICIENT 
Z  AXIS  SPLINE  COEFFICIENT 
NUMBER  OF  FILTER  STATES 
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GAMMA  OBSERVED  LINE-OF-SIGHT  ANGLE  (IN  PLANE) 

G2  GAUSSIAN  POINT 

H  STEP  SIZE 

I  ITERATION  COUNTER 

J  ITERATION  COUNTER 

JUP  UPPER  LIMIT  ON  'J'  ITERATION  COUNTER 

K  CONSTRAINT/COST  FUNCTION  MULTIPLIER 

KDEVF  CONSTRAINT  BASED  ON  FINAL  COVARIANCE 

KFLAG  KALMAN  GAIN  CONVERGENCE  FLAG 

MAXDV  MAXIMUM  INCREMENTAL  VELOCITY  CHANGE 

MAXG  MAXIMUM  THRUST  ACCELERATION  (G  FORCES) 

MDOT  UNITIZED  MASS  FLOW  RATE  OF  EVADER 

MDOTD  DUMMY  MASS  FLOW  RATE  OF  EVADER 

MDOTT  DUMMY  MASS  FLOW  RATE  OF  EVADER 

MEAN  MEAN  OF  MEASUREMENT  RESIDUALS 

MINDV  MINIMUM  INCREMENTAL  VELOCITY  CHANGE 

MING  MINIMUM  THRUST  ACCELERATION  (G  FORCES) 

MISS2  ESTIMATED  MISS  DISTANCE  SQUARED 

OPT  OPTION 

1  -  WITHOUT  KALMAN  FILTER 

2  -  WITH  KALMAN  FILTER 

3  -  WITH  KALMAN  FILTER  +  PRINTOUT 

PLAN  PLAN  OPTION 

1  -  PLAN  A 

2  -  PLAN  B 

3  -  PLAN  C 

4  -  DUAL  CONTROL 

5  -  CERTAINTY  CONTROL 

6  -  TRUTH  MODEL 

Q  PROPAGATION  NOISE  VARIANCE 

RANGE  RANGE  MEASUREMENT  OF  EVADER  FROM  PURSUER 

RES  MEASUREMENT  RESIDUALS 

RHO  CONTROL  EFFECTIVENESS  RATIO 

R3  MEASUREMENT  NOISE  VARIANCE 

SEED  RANDOM  NUMBER  SEED 

SFILTR  SIMULATION  FILTER 

SFLAG  SEARCH  CONVERGENCE  FLAG 

SIGMAM  STANDARD  DEVIATION  OF  MEASUREMENTS 

SIGTO  INITIAL  X,Y,Z  DEVIATIONS  AND  THEIR  RATES 

SIGTF  FINAL  X,Y,Z  DEVIATIONS  AND  THEIR  RATES 

SIMCNT  SIMULATION  COUNTER 

SKFLAG  SIMULATION  KALMAN  GIAN  CONVERGENCE  FLAG 

SNUM  SIMULATION  NUMBER 

SPLAN  SIMULATION  PLAN 

S RANGE  SIMULATION  RANGE 

S SFLAG  SIMULATION  SEARCH  CONVERGENCE  FLAG 

SVTOT  SIMULATION  TOTAL  VELOCITY  CHANGE 

SW  INTEGER  SWITCH  FOR  FUNCTION  'GAUSS' 

T  TIME 

TD  DUMMY  TIME 

TGO  TIME-TO-GO  (UNTIL  IMPACT) 

THETA  OBSERVED  LOS  ANGLE  (OUT  OF  PLANE) 

TMAT  TRANSFORMATION  MATRIX 

TOL  RANGE  TOLERANCE  FOR  SEARCH  ROUTINE 


ooooooooooooooooooonononoooooo 
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TSTART  START  TIME  FOR  CONTROL 

UPDATE  UPDATE  FLAG  FOR  EKF 

0  -  NO  UPDATE 

1  -  UPDATE 

2  -  UPDATE  WITH  RESIDUALS  SET  TO  ZERO 

VAR  VARIANCE  OF  MEASUREMENT  RESIDUALS 

VEL  EVADER  VELOCITY 

VTOT  TOTAL  VELOCITY  CHANGE 

XDUAL  DUMMY  XHAT  VECTOR 

XDUALD  DUMMY  XHAT  VECTOR 

XE  STATE  VECTOR  OF  EVADER 

XED  DUMMY  STATE  VECTOR  OF  EVADER 

XEDD  DUMMY  STATE  VECTOR  OF  EVADER 

XEEST  ESTIMATED  STATE  VECTOR  OF  EVADER 

XET  TRANSFORMED  STATE  VECTOR  OF  EVADER 

XHAT  ESTIMATED  STATES 

1  -  RELATIVE  X  POSITION 

2  -  RELATIVE  X  VELOCITY 

3  -  RELATIVE  Y  POSITION 

4  -  RELATIVE  Y  VELOCITY 

5  -  RELATIVE  Z  POSITION 

6  -  RELATIVE  Z  VELOCITY 

7  -  A 

8  -  MDOT 

XP  STATE  VECTOR  OF  PURSUER 

XPD  DUMMY  STATE  VECTOR  OF  PURSUER 

XPDD  DUMMY  STATE  VECTOR  OF  PURSUER 

XPEST  ESTIMATED  STATE  VECTOR  OF  PURSUER 

XPT  TRANSFORMED  STATE  VECTOR  OF  PURSUER 

XR  RELATIVE  EVADER  STATE  VECTOR 

DECLARE  VARIABLES 

REAL *8  A, MDOT, T, TIME, H , XE( 6 ) ,XP( 6 ) t XED( 6 ) ,XPD(6) 
REAL *8  TGO , MING , RES ( 3 ) , VAR (  3 )  , MEAN (  3 ) , XHAT ( 8 ) 
REAL* 8  DELTAY , DELTAZ , DV , K , DH , TD , THAT (3,3) , MAXG 
REAL *8  AX,BX,CX,DX,AY,BY,CY,DY,AZ,BZ,CZ,DZ,G2 
REAL *8  DDW, DDY , DDZ , XET( 6 ) , XPT( 6 ) , SIGMAM ( 3 ) , MISS2 
REAL* 8  COV (8,8),Q(8,8),R3(3,3) ,XEEST( 6 ) , XPEST( 6 ) 
REAL *8  MAXDV, MINDV, VTOT, XR(6) , RANGE, THETA, GAMMA 
REAL*8  MDOTD,MDOTT,SIGTO(6) ,SIGTF(6) 

REAL  *  8  GAUS  S , AD , AT , TOL , DUM , TSTART , KDEVF , RHO 
REAL *8  S RANGE , SVTOT , ASIGZ , BSIGZ , CSIGZ , DSIGZ 
REAL* 8  ASIGX,BSIGX, CSIGX, DSIGX , ASIGY , BSIGY , CSIGY 
REAL*8  DS I GY , CVD ( 6 ) , XPDD ( 6 ) , CVDD (8,8), XEDD ( 6 ) 

REAL *8  XDUAL ( 8 ) , XDUALD ( 8 ) , CVDUAL (8,8), DTGO 
INTEGER  I , J , JUP , COUNT , S I MCNT , SEED , OPT , SW , PLAN 
INTEGER  SNUM, UPDATE, SS FLAG 

INTEGER  KFLAG, SFLAG , FILTER, SPLAN, SFILTR, SKFLAG 
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Q  ******************* 

C  *  INITIALIZATIONS  * 

Q  ******************* 


READ  IN  INITIAL  CONDITIONS  FOR  DYNAMICS 
FORMAT ( 2X, 3F14 . 3 ) 

FORMAT (2X,F8. 2) 

FORMAT (2X,F9. 5) 

OPEN ( UNIT- 2 , NAME- ' [ ENGR . THESIS . SALFANO ] INIT . DAT' , 
+  TYPE- ' OLD ' , READONLY ) 

READ(2,1)XE(1) ,XE(3) ,XE(5) 

READ ( 2 , 1 } XE ( 2 ) ,XE(4) ,XE(6) 

READ ( 2 , 1 ) XP ( 1 ) , XP ( 3 ) , XP ( 5 ) 

READ ( 2 , 1 ) XP ( 2 ) ,XP( 4 ) ,XP( 6 ) 

READ( 2,6) TGO 
READ (  2 , 8  )  A 
READ ( 2,8 )MDOT 
CLOSE ( 2 ) 

PRINT  ' 

PRINT  ENTER  TIME  STEP  VALUE' 

READ  * , H 
PRINT  * , '  ' 

PRINT  ENTER  CONTROL/CONSTRAINT  MULTIPLIER' 

READ  * , K 
PRINT  ' 

PRINT  ENTER  CONTROL  EFFECTIVENESS  RATIO' 

READ  * , RHO 

IF  (RHO  .LT.  1.0)  RHO-l.O 

C  READ  IN  FILTER  MEASUREMENT  STANDARD  DEVIATIONS 
C  AND  ASSIGN  COVARIANCES  TO  R  MATRIX  DIAGONAL 
7  FORMAT ( F14 . 10 ) 

OPEN ( UNIT- 3 , NAME- » ( ENGR . THESIS . SALFANO ] 

+  FILTERS . REL ' , TYPE- ' OLD ' , READONLY ) 

READ (3,7) S IGMAM ( 1 ) 

READ ( 3 , 7 ) S I GMAM ( 2 ) 

READ( 3 , 7 )SIGMAM( 3 ) 

CLOSE ( 3 ) 

XR{ 1 )-XE( 1 )~XP( 1 ) 

XR( 3 )-XE( 3 )-XP( 3 ) 

XR( 5 )-XE( 5 )~XP( 5 } 

RANGE-SQRT(XR( 1 )*XR(1)+XR( 3)*XR(  3)+XR( 5)*XR(5) ) 

R3 ( 1 , 1 )-SIGMAM( 1 ) GRANGE* SI GMAM ( 1 ) * RANGE 
R3 ( 2 , 2 ) -SI GMAM ( 2 ) *SIGMAM( 2 ) 

R3( 3 , 3 ) -SI GMAM ( 3 ) *SIGMAM( 3 ) 

r  BPin  TM  THP  MPYT  cppn 

OPEN{ UNIT-4 , NAME- ' SIM . STATS ' , TYPE- ' OLD' , READONLY ) 

9  FORMAT ( 2X, F9 . 5 , 2X, F9 . 3 , 2 ( 2X, 12 ) , 2 ( 2X, 1 5 ) ) 

10  FORMAT (2X,I3,2X,I14) 

R£AD( 4,10} SNUM, SEED 
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PRINT 

*  r 

i 

PRINT 

*  f 

t 

WHAT 

FILTER  DO  YOU 

CHOOSE  ?' 

PRINT 

S' 

8 

-  EIGHT 

STATE 

EKF  ' 

PRINT 

S' 

6 

SIX 

STATE 

EKF  ' 

PRINT 
READ  * 

*, '  60 
, FILTER 

SIX 

STATE 

EKF  WITHOUT  GRAVITY 

C  ZERO  OUT  OFF  DIAGONAL  FILTER  MATRIX  COMPONENTS 
DO  40  1-2,8 
JUP-I-1 
DO  40  J«1,JUP 
COV(I, J)-0.0 
COV(J,I)«0.0 
Q( I , J )-0 . 0 
Q( J , I )»0 . 0 

40  CONTINUE 

C  ESTABLISH  ACCELERATION  AND 

C  MDOT  PROPAGATION  VARIANCES 
DUM- . l*MDOT*H 
Q( 8 , 8 )-DUM*DUM/H 
Q<7 , 7 ) -A*A*DUM*DUM*H 

C  COMPUTE  AND  INITIALIZE  PROPAGATION  VARIANCES 

C  COMPUTE  TRANSFORMATION  MATRIX 

CALL  COMPTV ( XP , TMAT ) 

C  TRANSFORM  ESTIMATED  STATE  VARIABLES 

CALL  TRANS FWD ( XP ( 1 ) ,XP(3) ,XP(5) , 

+  TMAT , XPT ( 1 )  , XPT ( 3 ) , XPT ( 5 ) ) 

CALL  TRANS FWD (XP( 2) ,XP(4) ,XP(6) , 

+  TMAT , XPT ( 2 ) , XPT ( 4 ) , XPT ( 6 ) ) 

CALL  TRANSFWD(XE( 1 ) ,XE( 3 ) ,XE{ 5 ) , 

+  TMAT , XET ( 1 ) , XET ( 3 ) , XET ( 5 ) ) 

CALL  TRANSFWD{ XE( 2 ) , XE( 4 ) ,XE( 6 ) , 

+  TMAT , XET ( 2 ) , XET { 4 ) , XET ( 6 ) } 

C  INITIALIZE  STATE  VECTORS  IN  NEW  FRAME 

DO  50  1-1,6 
XP( I ) -XPT( I ) 

XE( I ) -XET( I ) 

XED(I)-XET(I) 

50  CONTINUE 

C  ESTABLISH  DUMMY  TIME  STEP 

DH-H/256 

C  PROPAGATE  DUMMY  VARIABLES  FORWARD  ONE  STEP 
TD-0.0 
AD-A 

MDOTD-MDOT 
DO  60  1-1,256 

CALL  RK4SYSE(TD,XED,DH,AD,MDOTD) 

TD-TD+DH 

60  CONTINUE 
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C  PROPAGATE  TRANSFORMED  VARIABLES  FORWARD  ONE  STEP 

T-0.0 

IF  (FILTER  .EQ.  8)  THEN 
AT-A 

MDOTT-MDOT 

CALL  RK4SYSE(T,XET,H,AT,MDOTT) 

ENDIF 

IF  (FILTER  .EQ.  6)  THEN 
AT«A+SQRT(H*Q<7,7) ) 

MDOTT«MDOT+SQRT(H*Q(8,8) ) 

CALL  RK4SYSE(T,XET,H,AT,MD0TT) 

ENDIF 

IF  (FILTER  .EQ.  60)  THEN 
XET ( 1 ) -XET ( 1 ) +H*XET ( 2 ) 

XET ( 3 ) -XET( 3 ) +H*XET ( 4 ) 

XET( 5 ) -XET( 5 )+H*XET( 6 ) 

AT-A+SQRT( H*Q( 7,7) ) 

DUM-1 . 0+H*AT/SQRT(XET( 2 ) **2+XET( 4 ) **2+ 

+  XET( 6 )  **2 ) 

XET( 2 ) -XET( 2 ) *DUM 
XET ( 4 ) «XET{ 4 ) *DUM 
XET(6)-XET(6)*DUM 
ENDIF 

C  COMPUTE  REMAINING  Q  DIAGONAL  COMPONENTS 

Q( 1 , 1 )-( (XET(l)-XED(l) )**2+(XET(3)-XED(3) )**2+ 

+  ( XET ( 5 ) -XED (5))**2)/3. 0/H 

Q( 3 , 3  )«Q( 1,1) 

Q(5,5)-Q(l,l) 

Q(  2 , 2 )«( (XET( 2 )-XED( 2 ) ) **2+ (XET( 4 )-XED{ 4 ) )**2+ 

♦  ( XET (6) -XED (6) )**2)/3. 0/H 

Q(4,4)-Q(2,2) 

Q( 6 ,6 )-Q{ 2,2) 

Q { 7  #  7 ) ■ ( AT -AD ) * ( AT-AD ) /H 

C  ASSIGN  STARTUP  COVARIANCES 
COV(1,1)-100.0 
COV{  2 , 2  ) -SQRT( COV(  1,1)  ) 

COV (3,3) -COV (1,1) 

COV (4,4) -COV (2,2) 

COV (5,5) “COV  (1,1) 

COV(6,6)-COV(2,2) 

COV(7,7)-(0.1*A)**2 
COV( 8 ,8 )-( 0 . l*MDOT) **2 

C  INITIALIZE  VARIABLES 
TOL-0.0001 
TS TART- 3.0 
SFLAG-0 
KFLAG-0 
SW-0 

DELTAY-0 . 0 
DELTAZ-0 . 0 
VTOT-O . 0 
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MAXG-6.0 
MING-O. 05*MAXG 
MAXDV-MAXG*10 .  0*H 
MINDV-MING*10.0*H 
COUNT-lOO 


C  ASK  USER  TO  CHOOSE  CONTROL  METHOD 


PRINT  * 

PRINT  * 

PRINT  * 

PRINT  * 

PRINT  * 

PRINT  * 

PRINT  * 

PRINT  * 

READ  * , PLAN 
UPDATE-1 
IF  (PLAN  .EQ. 
IF  (PLAN  .EQ. 


WHAT  CONTROL  METHOD  DO  YOU  CHOOSE  ? ' 


PLAN  A' 

PLAN  B' 

PLAN  C' 

DUAL  CONTROL' 
CERTAINTY  CONTROL' 
TRUTH  MODEL' 


4)  UPDATE-2 

5)  UPDATE-0 


C  ASK  USER  FOR  NOISE  OPTION 


PRINT  * 
PRINT  * 
PRINT  * 
PRINT  * 
PRINT  * 
PRINT  * 
READ  *,OPT 


CHOOSE  YOUR  OPTION' 

1  -  NO  NOISE' 

2  -  NOISE' 

3  -  NOISE  +  SCREEN  PRINTOUT' 

4  -  NOISE  +  DATAFILE  PRINTOUT' 


C  INITIALIZE  ESTIMATED  VARIABLES 
DO  100  1-1,6 
XPEST{ I ) -XP ( I ) 

IF  (OPT  .EQ.  1)  THEN 
XEEST(I)-XE(I) 

ELSE 

XEEST(I)-XE(I)+SQRT(COV(I,I) )* 

+  GAUSS ( SEED, SW,G2) 

ENDIF 

XHAT( I )-XEEST( I )-XPEST( I ) 

100  CONTINUE 

IF  (OPT  .EQ.  1)  THEN 
XHAT( 7 )-A 
XHAT( 8 )-MDOT 
ELSE 

XHAT( 7 ) -A+SQRTt  COV( 7,7)) * GAUSS ( SEED , SW, G2 ) 
XHAT( 8 ) -MDOT+SQRT (  COV (8,8)) * GAUSS ( SEED , SW , G2 ) 
ENDIF 

C  OPEN  UNITS  FOR  WRITING  OUTPUT  DATA 
5  FORMAT( F13 . 7 , 2X, FI  3 . 7 ) 

IF  (OPT  .EQ.  4)  THEN 

OPEN( UNIT-11,  FILE- 'RES1.DAT' , STATUS-' NEW' , 

+  IOSTAT-ISTAT) 
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OPEN ( UNIT® 12 , FILE- 
+  IOSTAT-ISTAT) 

OPEN  (  UNIT=1 3  ,  FILE- 
+  IOSTAT-ISTAT ) 

OPEN (UNIT-1 4, FILE- 
+  IOSTAT-ISTAT ) 

OPEN( UNIT-15 , FILE- 
+  I OS TAT- I S TAT ) 

OPEN( UNIT- 16 , FILE- 
+  IOSTAT-I STAT ) 

OPEN(UNIT-17 , FILE* 
+  IOSTAT-I STAT) 

OPEN( UNIT-18 , FILE- 
+  IOSTAT-ISTAT ) 

OPEN(UNIT-19 , FILE- 
+  IOSTAT-ISTAT) 

OPEN (UNIT-20,  FILE- 
+  IOSTAT-ISTAT) 

OPEN(UNIT-21 , FILE- 
+  IOSTAT-ISTAT) 

OPEN( UNIT-22  , FILE- 
+  IOSTAT-ISTAT) 

OPEN( UNIT-23 , FILE* 
+  IOSTAT-ISTAT) 

OPEN( UNIT-24, FILE- 
+  IOSTAT-ISTAT ) 

OPEN< UNIT-25, FILE- 
+  IOSTAT-ISTAT) 

OPEN( UNIT-26, FILE- 
+  IOSTAT-ISTAT) 

OPEN (UNIT-27,  FI LE- 
+  IOSTAT-ISTAT) 

OPEN( UNIT-28,  FILE¬ 
S'  IOSTAT-ISTAT) 

OPEN (UNIT-29,  FILE¬ 
S'  IOSTAT-ISTAT) 

OPEN(UNIT-30, FILE¬ 
S'  IOSTAT-ISTAT) 

OPEN(UNIT-31, FILE¬ 
S'  IOSTAT-ISTAT) 

OPEN(UNIT-32, FILE¬ 
S'  IOSTAT-ISTAT) 

OPEN(UNIT-33, FILE¬ 
S'  IOSTAT-ISTAT) 

OPEN(UNIT-34, FILE¬ 
S'  IOSTAT-ISTAT) 

OPEN(UNIT-35, FILE¬ 
S'  IOSTAT-ISTAT) 

OPEN( UNIT-36,  FILE¬ 
S'  IOSTAT-ISTAT) 

OPEN( UNIT-37,  FILE¬ 
S'  IOSTAT-ISTAT) 

OPEN(UNIT-38, FILE¬ 
S'  IOSTAT-ISTAT) 

END  IF 


'  RES2 . DAT ' , STATUS- ' NEW ' , 

'  RES  3 . DAT ' , STATUS- ' NEW ' , 

'  RES 4 . DAT' , STATUS- 'NEW' , 

' RES5.DAT' , STATUS- 'NEW' , 
'RES6.DAT' , STATUS* 'NEW' , 

'  RES 7 . DAT ' , STATUS- 'NEW' , 
'RES8.DAT' , STATUS- 'NEW' , 

'  DELTAY.DAT' , STATUS- ' NEW' , 
'DELTAZ.DAT' , STATUS- 'NEW' , 
'COVl.DAT' , STATUS- 'NEW' , 

'  COV2 . DAT ' , STATUS- ' NEW ' , 

'  COV3 . DAT ' , STATUS- ' NEW ' , 

' COV4 . DAT ' , STATUS- ' NEW ' , 

'  COV5 . DAT ' , STATUS- ' NEW ' , 

' COV6 . DAT ' , STATUS- ' NEW ' , 

' COV7 . DAT ' , STATUS- ' NEW' , 

' COV8 . DAT ' , STATUS- ' NEW ' , 
'MISS. DAT ' , STATUS- ' NEW ' , 

' TOL . DAT ' , STATUS- 'NEW' , 

' COVlM . DAT ' , STATUS- ' NEW ' , 

' COV2M . DAT ' , STATUS- ' NEW ' , 

' COV3M.DAT' , STATUS- 'NEW' , 
'COV4M.DAT' , STATUS- 'NEW' , 
'COV5M.DAT' , STATUS- 'NEW' , 

' COV6M . DAT ' , STATUS- ' NEW ' , 
'COV7M.DAT' , STATUS- 'NEW' , 

' COV8M . DAT ' , STATUS- ' NEW ' , 
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Q  ************************* 

C  *  BEGIN  SIMULATION  LOOP  * 

Q  ************************* 


DO  990  SIMCNT-1, 50000 

C  PRINT  RESIDUALS,  VELOCITY  CHANGES, 

C  AND  COVARIANCES  TO  DATAFILES 
IF  (OPT  .EQ.  4)  THEN 

WRITE (11, 5) ,T,XEEST( 1 )-XE(l ) 

WRITE ( 12,5) , T,XEEST( 2 )-XE( 2 ) 

WRITE ( 13,5) , T , XEEST ( 3 ) -XE ( 3 ) 

WRITE (14, 5) , T, XEEST ( 4 ) -XE( 4 ) 

WRITE (15, 5) , T, XEEST ( 5)-XE( 5 ) 

WRITE ( 16,5) , T, XEEST ( 6 ) -XE( 6 ) 

WRITE( 17,5) , T , XHAT ( 7 ) -A 
WRI TE ( 1 8 , 5 ) , T , XHAT ( 8 ) -MOOT 
IF  ((DELTAY  .NE.  0.0)  .OR.  (SIMCNT  .EQ.  1)) 
+  WRI TE ( 1 9 , 5 ) , T , DELTAY 

IF  ((DELTAZ  .NE.  0.0)  .OR.  (SIMCNT  .EQ.  1)) 
+  WRITE (20,5) , T , DELTAZ 

WRITE(21,5) ,T,SQRT(COV(l,l) ) 

WRI TE ( 2  2 , 5 ) , T , SQRT ( COV ( 2 , 2 ) ) 

WRITE ( 23,5) ,T, SQRT( COV( 3,3)) 

WRI TE ( 2  4 , 5 } , T , SQRT ( COV (4,4) ) 

WRITE ( 25,5) , T, SQRT ( COV ( 5,5) ) 

WRITE (26, 5) ,T, SQRT (COV (6, 6) ) 

WRITE( 27,5) , T, SQRT( COV( 7,7)) 

WRITE( 28 ,5) , T , SQRT ( COV (8,8)) 

WRITE( 29,5) ,T, SQRT ( MIS S2 ) 

IF  ( KDEVF  .GT.  0.0)  WRITEt 30, 5 ) ,T,KDEVF 
WRITE( 31,5) ,T,-SQRT(COV( 1,1) ) 

WRITE ( 32,5) , T, -SQRT{ COV( 2,2)) 

WRITE{ 33,5) , T, -SQRT( COV( 3,3)) 

WRITE ( 34,5) ,T, -SQRT( COV( 4,4)) 

WRITE ( 35,5) ,T, -SQRT( COV( 5,5)) 

WRITE( 36,5) ,T, -SQRT( COV( 6,6)) 

WRITE ( 37,5) , T, -SQRT( COV( 7,7)) 

WRITE( 38,5) ,T,-SQRT(COV( 8,8) ) 

ENDIF 

C  TEST  TO  SEE  IF  TIME  IS  UP 

IF  (TOO  .LE.  H)  GOTO  995 

C  REASSIGN  DUMMY  VARIABLES 
DO  105  1-1,6 
XPD(I)-XPEST(I) 

XED( 1 ) -XEEST ( I ) 

105  CONTINUE 
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IF  ((UPDATE  .NE.  1)  .AND.  (T  .GE.  TRTART) )  THEN 
DO  106  1-1,8 

XDUAL ( I } -XHAT ( I ) 

DO  106  J*l,8 

CVDUAL ( I , J ) -COV ( I , J ) 

106  CONTINUE 

ENDIF 

C  PROPAGATE  DUMMY  VARIABLES  FORWARD  ONE  STEP 
TD— T 

IF  ((UPDATE  .NE.  1)  .AND.  (T  .GE.  TSTART) )  THEN 
RANGE-SQRT ( XDUAL ( 1 ) * XDUAL ( 1 ) + XDUAL ( 3 ) *XDUAL ( 3 ) + 
+  XDUAL ( 5 ) *XDUAL ( 5 ) ) 

R3 ( 1 , 1 )-SIGMAM( 1 ) *RANGE*SIGMAM( 1 ) * RANGE 
IF  (FILTER  .EQ.  8)  CALL  EKF8 ( XDUAL , XED , XPD , TD , 

+  H , CVDUAL ,Q,R3, 0.0,0. 0,0.0, KFLAG , RES , UPDATE ) 

IF  (FILTER  .EQ.  6)  THEN 

Q{ 2 , 2 ) -1 . 0+XDUAL( 7 ) *H*Q( 8 , 8 ) 

Q(4,4)-Q(2,2) 

Q( 6 , 6 )«Q( 2 , 2 ) 

CALL  EKF6 ( XDUAL , XED , XPD , TD , H , CVDUAL , Q , 

+  R3, 0.0, 0.0, 0.0, KFLAG , RES , UPDATE ) 

ENDIF 

AD-XDUAL ( 7 ) 

MDOTD-XDUAL ( 8 ) 

ELSE 

AD-XHAT( 7 ) 

MDOTD-XHAT( 8 ) 

CALL  RK4SYSP( TD,XPD,H) 

CALL  RK4 S YSE ( TD , XED , H , AD , MDOTD ) 

ENDIF 

TD-TD+H 

TGO-TGO-H 

C  INITIALIZE  TRANSFORMED  VARIABLES  TO  DUMMY  VARIABLES 
AT-AD 

J1DOTT-MDCTD 
DO  110  1-1,6 
XET( I )-XED( I ) 

XPT{ I )«XPD( I ) 

110  CONTINUE 

IF  (PLAN  .EQ.  4)  THEN 
DO  111  1-1,8 

XDUALD ( I ) -XDUAL ( I ) 

DO  111  0-1,8 

CVDD( I , 0 }»CVDUAL< 1,0) 

111  CONTINUE 

DO  112  1-1,6 
XEDD( I ) >XED( I ) 

XPDD( I )-XPD( I ) 

112  CONTINUE 
ENDIF 
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C  STORE  INITIAL  VALUED  OF  DEVIATIONS  FOR 
C  SPLINE  COMPUTATION 

IF  {(PLAN  .EQ.  5)  .AND. 

+  (T  .GE.  TSTART) )  THEN 

SIGTO { 1 ) -SQRT( CVDUAL (1,1)  ) 

SIGTO (2)-(SIGT0(l) -SQRT( COV( 1,1) ) )/H 
SIGTO ( 3 ) *SQRT( CVDUAL( 3,3) ) 

SIGTO {  4 )  -  {  SIGTO  (  3  ) -SQRT(  COV(  3,3)  )  )/H 
SIGTO ( 5 )=SQRT( CVDUAL( 5,5) ) 

SIGTO  (  6  ) * (  SIGTO  (  5  ) -SQRT'(  COV(  5,5))  )/H 
END  IF 

C  ESTABLISH  SPLINE  TIME  STEP 

IF  l TGO/COUNT  .LT.  H)  COUNT-COUNT- 1 
IF  (COUNT  .LT.  1)  COUNT-1 
DH-TGO/CDUNT 

C  PROPAGATE  DUMMY  VARIABLES  FORWARD 
C  TO  PREPIC. -D  IMPACT  TIME 

IF  ( ( PLAN  . EQ .  5)  .AND. 

+  (T  .GE.  TSTART))  THEN 

DO  113  1-1 , COUNT 

IF  (I  .EQ.  COUNT)  THEN 
CVD ( 1 ) -CVDUAL (1,1) 

CVD ( 3 ) -CVDUAL (3,3) 

CVD  {  5  )  *-  CVDUAL  (5,5) 

END  IF 

RANGE- SQRT( XDUAL ( 1 ) * XDUAL ( 1 ) +XDUAL ( 3 ) * 

+  XDUAL ( 3 ) +XDUAL ( 5 ) * XDUAL ( 5 ) ) 

R3 ( 1 , 1 )-SIGMAM( 1 ) *  RANGE  *  S I GMAM { 1 ) * RANGE 
IF  (FILTER  .EQ.  8)  THEN 

CALL  EKF8 ( XDUAL , XED , XPD , TD , DH , CVDUAL , Q , R3 , 
+  0.0, 0.0*0. 0r KFLAG , RES , UPDATE ) 

ENDIF 

IF  (FILTER  .EQ.  6)  THEN 

Q(2,2)-1.0+XDUAL(7)*DH*Q(8,8) 

Q( 4 , 4 )»Q( 2 , 2 ) 

Q( 6 ,6 )-Q( 2,2 ) 

CALL  EKF6 ( XDUAL , XED , XPD , TD , DH  f CVDUAL , Q , 

+  R3, 0.0, 0.0, 0.0, KFLAG, RES, UPDATE) 

ENDIF 
TD-TD+DH 

113  CONTINUE 

SIGTP(1)-SQRT( CVDUAL  (1,1) ) 

SIGTF ( 2 ) - ( b IGTF( 1 ) -SQRT( CVD( 1 ) ) )/DH 
SIGTF( 3 )«SQRT( CVDUAL { 3,3) ) 

SIGTF (  4  )-(  SIGTF (  3  )-SQRT(CVD(  3  )  )  )/'DH 
SIGTF ( 5 ) -SQRT( CVDUAL ( 5,5) ) 

SIGTF( 6 )-( SIGTF ( 5 )-SQRT{ CVP( 5 ) ) )/DH 
KDEVF-SQRT ( K* ( CVDUAL (1,1) + CVDUAL (3,3)+ 

+  CVDUAL ( 5,5))) 

ELSE 


u  u 
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DO  115  I -1, COUNT 

CALL  RK4SYSP ( TD , XPD , DH ) 

CALL  RK 4 S YSE ( TD , XED , DH , AD , MDOTD ) 

TD-TD+DH 

115  CONTINUE 

ENDIF 

C  COMPUTE  FINAL  ESTIMATED  RELATIVE  STATES 
DO  120  1*1,6 

XR ( I ) «XED ( I } -XPD  ( I ) 

120  CONTINUE 

C  COMPUTE  ESTIMATED  MISS  DISTANCE  SQUARED 

MISS2«XR( 1 ) *XR ( 1 ) +XR( 3 ) *XR( 3 )+XR( 5 ) *XR( 5) 

C  UPDATE  DUMMY  TIME-TO-GO 
DTGO-TD-T-H 

C  COMPUTE  VELOCITY  CHANGES 

IF  { ( TGO  .GT.  H)  .AND. 

+  (T  .GE.  TSTART) )  THEN 

C  COMPUTE  RELATIVE  SPLINE  COEFFICIENTS 

CALL  SPLINE (XET( 1 )-XPT( 1 ) , XET( 2 )-XPT< 2 ) ,XR(1) , 
+  XR ( 2 } , DTGO , AX , BX , CX , DX ) 

CALL  SPLINE (XET( 3 )-XPT( 3 ) ,XET( 4 )-XPT( 4 ) ,XR( 3 ) , 
+  XR ( 4 ) , DTGO , AY , B Y , CY , DY ) 

CALL  SPLINE ( XET( 5 ) -XPT( 5 ) , XET( 6 )-XPT( 6 ) , XR( 5 ) , 
+  XR(6) ,DTGO,AZ,BZ,CZ,DZ) 

IF  (PLAN  .EQ.  5)  THEN 

CALL  SPLINE ( SIGTO ( 1 ) , SIGTO ( 2 ) ,SIGTF{1) , 

•f  SIGTF  (  2  )  ,  DTGO ,  ASIGX ,  BSIGX ,  CSIGX ,  DSIGX ) 

CALL  SFLINE( SIGT0{ 3 ) , SIGTO (  4  )  , SIGTF ( 3 ) , 

+  SIGTF ( 4 ) , DTGO , AS I GY , BSIGY , CSIGY, DSIGY ) 

CALL  SPLINE ( SIGTO (  5 )  , SIGTO ( 6 ) , SIGTF ( 5 ) , 

+  SIGTF ( 6 ) , DTGO, AS I GZ , BSIGZ , CSIGZ , DSIG7 ) 

ENDIF 

COMPUTE  OPTIMAL  CHANGES  IN  VELOCITY  AND 
IMPACT  TIME 
IF  (PLAN  .EQ.  1)  THEN 

CALL  SEARCHA ( AX , BX , CX , DX , AY , BY , CY , DY , AZ , BZ , 

+  CZ , DZ , K , TGO , DELTAY , DELTAZ , TOL , SFLAG ) 

ENDIF 

IF  (PLAN  .EQ.  2'  THEN 

CALL  SEARCHB ( AX , BX , CX , DX , AY , B Y 4 CY , DY , AZ , BZ , 
+  CZ,DZ,TGO, DELTAY , DELTAZ , TOL , SFLAG ) 

ENDIF 
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IF  (PLAN  .EQ.  3)  THEN 
DO  135  1-1,6 

XR( I )=XET( I )-XPT( I ) 

135  CONTINUE 

CALL  SEARCHC ( XR , XET( 2 ) ,XET(4) ,XET(6) ,  AT  , 

+  MDOT , TOO , DELTAY , DELTAZ , TOL , SFLAG ) 

ENDIF 

IF  (PLAN  .EQ.  4)  THEN 

CALL  SEARCHD ( AX , BX , CX , DX , AY , BY , CY , DY , AZ , BZ , 

+  CZ , DZ , K , TGO , DELTAY , DELTAZ , TOL , SFLAG , 

+  XDUALD , XEDL , XPDD , CVDD , MAXDV , COUNT , 

+  Q,R3 , SIGMAM,H) 

ENDIF 

IF  (PLAN  .EQ.  5)  THEN 

CALL  SEARCHCC ( AX , BX , CX , DX , AY , B Y , CY , DY , AZ , BZ , 
+  CZ , DZ , ASIGX, BSIGX , CSIGX, DSIGX, ASIGY, BSIGY , 

+  CSIGY, DSIGY , ASIGZ ,BSIG2 , CSIGZ , DSIGZ , K,TGO, 

+  DELTAY, DELTAZ, TOL, SFLAG) 

ENDIF 

IF  (PLAN  .EQ.  6)  THEN 

CALL  SEARCHT ( XPT , XET , AT , MDOTT , H , T, TGO , 

+  DELTAY , DELTAZ , TOL , S  FLAG , COUNT ) 

ENDIF 

C  BOUND  VELOCITY  CHANGES 

IF  (ABS( DELTAY)  .LT.  MINDV)  THEN 
DELTAY-0 . 0 
ELSE 

IF  (DELTAY  .GT.  MAXDV)  DEL T A Y - MAXDV 
IF  (DELTAY  .  LT .  -MAXDV)  DELTAY— MAXDV 
ENDIF 

IF  (ABS( DELTAZ)  .LT.  MINDV)  THEN 
DELTA2-Q . 0 
ELSE 

IF  (DELTAZ  .GT.  MAXDV)  DELTAZ -MAXDV 
IF  (DELTAZ  .LI.  -MAXDV)  DELTAZ— MAXDV 
ENDIF 

DV-ABS ( DELTAY ) +ABS ( DELTAZ ) 

IF  (DV  .GE.  MAXDV)  THEN 
TSTART-T 
ELSE 

TSTART-T+ ( RHC-1 . 0  5  *TGO/RHO 
ENDIF 

ELSE 

DELTAY-0. 0 
DELTAZ-0.0 
DV-0 . 0 


END  IF 


IF  (DV  .GE.  MINDV)  WRITE (*, 1 ) DELTAY , DELTAZ , TGO 

C  PROPAGATE  REAL  VARIABLES  AND  PURSUER  ESTIMATE 
C  FORWARD  ONE  STEP  (I.M.U.  ASSUMED  PERFECT) 

CALL  RK4SYSP(T,XP,H) 

CALL  RK4SYSE(T,XE,H,A,MDOT) 

DO  210  1-1,6 
XPEST( I )-XP( I ) 

210  CONTINUE 

C  COMPUTE  SENSOR  MEASUREMENTS  (PLUS  NOISE) 

XR ( 1 ) -XE ( 1 ) -XP ( 1 ) 

XR( 3 ) -XE ( 3 ) -XP ( 3 ) 

XR ( 5 ) -XE ( 5 ) -XP ( 5 ) 

RANGE«SQRT(XR( 1 ) *XR( 1 )+XR( 3 ) *XR( 3 )+XR( 5 ) *XR( 5 ) ) 
R3 ( 1 , 1 ) -RANGE*SIGMAM( 1 ) *RANGE*SIGMAM( 1 ) 
THETA-ATAN ( XR ( 5 ) /XR ( 1 ) ) 

GAMMA -ATAN ( XR ( 3 ) /XR ( 1 ) ) 

IF  (OPT  .NE.  1)  THEN 

RANGE-RANGE* ( 1 . 0+GAUSS ( SEED , SW, G2 ) *SIGMAM( 1 ) ) 
THETA-THETA+GAUSS ( SEED , SW, G2 ) *SIGMAM( 2 ) 
GAMMA-GAMMA+GAUSS ( SEED , SW , G2 ) *SIGMAM( 3 ) 

END  IF 

C  GET  FILTER  ESTIMATES 

IF  (FILTER  .EQ.  8)  CALL  EKF8 ( XHAT , XEEST , XPEST , 

+  T , H , COV , Q , R3 , RANGE , THETA , GAMMA , KFLAG , RES , 1 ) 

IF  (FILTER  .EQ,  6)  THEN 

Q( 2 , 2 )-l . 0+XHAT( 7 ) *H*Q( 8 , 8 ) 

Q(4,4)-Q(2,2) 

Q(6,6)-Q(2,2) 

CALL  EKF6( XHAT, XEEST, XPEST, T,H, 

+  COV, Q,R3, RANGE, THETA, GAMMA, KFLAG, RES, 1) 

END  IF 

IF  (FILTER  .EQ.  60)  CALL  EKF60 ( XHAT , XEEST, 

+  XPEST, T,H, COV, Q,R3, RANGE, THETA, GAMMA, KFLAG, RES) 

C  UPDATE  EVADER  ESTIMATE  USING  RELATIVE  ESTIMATE 
DO  200  1-1,6 

XEEST{ I ) -XPEST ( I )+XHAT( I ) 

200  CONTINUE 

C  RECURSIVELY  COMPUTE  MEAN  AND  VARIANCE  OF 
C  MEASUREMENT  RESIDUALS 

IF  ( SIMCNT  .EQ.  1)  THEN 
VAR( 1 ) -RES ( 1 ) *RES ( 1 ) 

VAR( 2 ) -RES ( 2 ) *RES  (  2 ) 

VAR( 3 ) -RES ( 3 ) *RES  (  3 ) 

ELSE 

VAR( 1 )-VAR( 1 ) * ( SIMCNT-2 )/( SIMCNT-1 )+ 

+  { MEAN ( 1 ) -RES (!))*( MEAN ( 1 ) -RES ( 1 )  J/SIMCNT 
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VAR( 2 ) -VAR( 2 ) * ( SIMCNT-2 ) /( SIMCNT-1 ) + 

+  ( MEAN ( 2 ) -RES ( 2 ) ) * ( MEAN( 2 ) -RES ( 2 ) )/SIMCNT 

VAR ( 3 ) * VAR ( 3 ) * ( SIMCNT-2 )/( SIMCNT-1 ) + 

+  ( MEAN ( 3 ) -RES ( 3 ) ) * ( MEAN ( 3 ) -RES ( 3 ) ) /S IMCNT 

END  IF 

MEAN(1)«(MEAN(1)*( SIMCNT-1 )+RES(l) )/SIMCNT 
MEAN ( 2 ) - ( MEAN ( 2 ) * ( SIMCNT-1 ) +RES ( 2 ) ) /SIMCNT 
MEAN( 3 ) - ( MEAN ( 3 ) * ( SIMCNT-1 ) +RES ( 3 ) )/SIMCNT 

C  PRINT  ESTIMATED  AND  TRUE  STATES  AND  COVARIANCES 
IF  (OPT  .EQ.  3)  THEN 
PRINT  *, '  ' 

PRINT  ESTIMATED  STATE,  TRUE  STATE,  ERROR' 

PRINT  *,'  AND  COVARIANCE' 

DO  220  1-1,6 

PRINT  * , XHAT ( I ) , XE ( I ) -XP ( I ) , 

+  XHAT ( I ) -XE ( I ) +XP ( I ) 

PRINT  ' , COV (1,1) 

220  CONTINUE 

END  IF 

C  APPLY  VELOCITY  CHANGES 
XP ( 4 ) -XP ( 4 ) +DELTAY 
XP ( 6 ) -XP ( 6 ) +DELTAZ 
XPEST( 4 )-XP( 4 ) 

XPEST( 6 )-XP( 6 ) 

XHAT ( 4 ) -XHAT ( 4 ) -DELTAY 
XHAT ( 6 ) -XHAT( 6 ) -DELTAZ 
VTOT-VTOT+DV 

C  UPDATE  TIME 
T-T+H 

990  CONTINUE 


£  *********************** 
C  *  END  SIMULATION  LOOP  * 
£  *********************** 


995  CONTINUE 

C  PRINT  SQUARE  ROOT  OF  COVARIANCE  DIAGONAL 
PRINT  *, '  ' 

PRINT  DEVIATIONS,  ERROR' 

DO  800  1-1,6 

PRINT  *,SQRT(COV(I,I) ) ,XEEST( I )-XE( I ) 
800  CONTINUE 

PRINT  * , SQRT ( COV (7,7)), XHAT ( 7 ) -A 
PRINT  * , SQRT( COV (8,8)) ,XHAT( 8 ) -MDOT 
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C  PROPAGATE  REAL  DATA  TO  FINAL  PREDICTED  IMPACT  TTME 
DH-TGO 

CALL  RK4SYSP( T, XP , DH ) 

CALL  RK4SYSE ( T,XE , DH , A, MDOT ) 

T-T+DH 

C  ITERATE  TO  FIND  POINT  OF  CLOSEST  APPROACH 
DH-H 

DO  310  J-1,25 
DO  300  1-1,6 

XR ( I ) -XE ( I ) -XP ( I ) 

300  CONTINUE 

IF  (ABS(DH)  .LE.  0.0000001)  GOTO  320 
DH-XR( 1 ) *XR( 2 ) +XR( 3 ) *XR( 4 )+XR( 5 ) *XR( 6 ) 

DH— DH/(XR(2)*XR(2)+XR(4  ) *XR( 4 )+XR( 6 ) *XR( 6 )  ) 
CALL  RK4SYSP ( T , XP , DH ) 

CALL  RK4  SYSE ( T , XE , DH , A , MDOT ) 

T-T+DH 
310  CONTINUE 

320  CONTINUE 

C  PRINT  CONVERGENCE  MESSAGE 
PRINT  * ,  '  ' 

PRINT  SEARCH  NON-CONVERGENCE  -' , SFLAG 

PRINT  GAIN  NON-CONVERGENCE  - ' , KFLAG 

PRINT  *, •  • 

C  PRINT  TIME  AND  MISS  OF  CLOSEST  APPROACH 

PRINT  IMPACT  TIME  :  ',T 

RANGE-SQRT ( XR ( 1 ) *XR( 1 )+XR( 3 ) *XR( 3 )+XR( 5 ) *XR( 5 ) ) 
PRINT  TOTAL  VELOCITY  CHANGE  :  ' ,VTOT 
PRINT  *,•  MISS  DISTANCE  :  RANGE 

PRINT  * , XR ( 1 ) , XR ( 3 ) , XR ( 5 ) 

C  CLOSE  OUTPUT  DATA  FILES 
IF  (OPT  .EQ.  4)  THEN 
DO  340  1-11,38 
CLOSE(I) 

340  CONTINUE 
ENDIF 

C  READ  IN  PREVIOUS  SIMULATION  DATA,  SORT  ON  RANGE, 

C  AND  WRITE  TO  NEW  FILE 

OPEN (UNIT- 5, FILE- 'SIM. STATS ' , STATUS- 'NEW' , 

+  IOSTAT-ISTAT) 

WRITE( 5 , 1 0 ) ( SNUM+1 ) , SEED 
SRANGE-0 . 0 

C  RANGE-SIGN( RANGE, XR( 3 ) ) 

J-0 
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DO  350  I“1 , SNUM 

READ (  4 , 9  )  S  RANGE , S VTOT , S  P  LAN ,SFILTR,SSF  LAG , S  K  F LAG 
IF  ((J  .EQ.  0)  .AND.  (RANGE  . LE .  SRAFGE ) ) THEN 
WRI TE ( 5 , 9 ) RANGE , VTOT , PLAN . FILTER , SFLAG , KFLAG 
J-l 
END  IF 

WRITE (5,9) S RANGE , SVTOT , SPLAN , SFILTR , SSFLAG , SKFLAG 
350  CONTINUE 

IF  (J  .EQ.  0) 

+  WRITE( 5,9 )RANGE, VTOT, PLAN,FILTER, SFLAG, KFLAG 

CLOSE( 4  ) 

CLOSE( 5 ) 

END 


onoooonnooo 
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C  TOOLl 

C  THIS  IS  A  COLLECTION  OF  SUBROUTINES  NEEDED  FOR 
C  ORBIT  PROPAGATION 

C  IN  THE  HYPERVELOCITY  ORBITAL  INTERCEPT  PROGRAM 

C  SUBROUTINE  DICTIONARY 

A  EVADER  ACCELERATION  DUE  TO  THRUSTING 

AD  DUMMY  ACCELERATION 

COUNT  ITERATION  FINAL  COUNT 

MDOT  UNITIZED  MASS  FLOW  RATE  OF  EVADER 

MDOTD  DUMMY  MASS  FLOW  RATE  OF  EVADER 

H  STEP  SIZE 

I  ITERATION  COUNTER 

T  TIME 

TFINAL  FINAL  TIME 

XE  STATE  VECTOR  OF  EVADER 

XP  STATE  VECTOR  OF  PURSUER 


SUBROUTINE  XPSYSP(X,F) 

C  THIS  SUBROUTINE  EVALUATES  THE  FUNCTIONS  FOR  RK4SYSP 
C  (ORBITAL  DYNAMICS  FOR  TWO  BODY  PROBLEM  FOR  PURSUER) 
C  (X  VECTOR  -  (X  XDOT  Y  YDOT  Z  ZDOT) ) 

C  (F  VECTOR  »  [XDOT  XDOUBLEDOT  YDOT  YDOUBLEDOT 
C  ZDOT  ZDOUBLEDOT]) 

REAL *8  X ( 6 ) , F ( 6 ) , RSQRD , CONST 

RSQRD«X{ 1 ) *X( 1 ) +X( 3 ) *X{ 3 )+X( 5 ) *X( 5 ) 

CONST— 3 . 9 8601 2E14/RSQRD/SQRT(  RSQRD) 

F(1)«X(2) 

F(2)-CONST*X(l) 

F ( 3 ) -X ( 4 ) 

F(4)-CONST*X(3) 

Ft  5)-X{ 6) 

F(6)«CONST*X( 5) 

END 


SUBROUTINE  RK4SYSP(T,X,H) 

C  THIS  SUBROUTINE  IS  A  ONE  STEP  RUNGE-KUTTA 
C  4TH  ORDER  INTEGRATOR  FOR  THE  PURSUER  DYNAMICS 
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REAL *8  T,X(6),H,Fl(6),F2(6),F3(6) 

REAL *8  F4(6) ,H2,DUMX(6) 

INTEGER  I 

H2-0 . 5*H 

C  FIND  Fl 

CALL  XPSYSP* X,Fl) 

C  FIND  F2 

DO  100  1-1,6 

DUMX( I )*X( I )+H2*Fl  ( I ) 

100  CONTINUE 

CALL  XPSYSP ( DUMX , F2 ) 

C  FIND  F3 

DO  200  1-1,6 

DUMX( I ) -X( I ) +H2  *F2 ( I ) 

200  CONTINUE 

CALL  XPSYSP ( DUMX , F3 ) 

C  FIND  F4 

DO  300  1-1,6 

DUMX(  I )  -X(  I  )+H*F3  ( I ) 

300  CONTINUE 

CALL  XPSYSP ( DUMX ,F4) 

C  UPDATE  THE  STATE 
DO  400  1-1,6 

X( I ) -X( I )+H* (F1(I)+F2{I)+F2(I)+F3(I)+ 
+  F3 ( I ) +F4 ( I ) )/6 . 0 

400  CONTINUE 

END 


SUBROUTINE  EULERP(T,X,H) 

C  THIS  SUBROUTINE  IS  A  ONE  STEP  EULER  INTEGRATOR 
C  FOR  THE  PURSUER  DYNAMICS 

REAL* 8  T,X(6),F1(6),H 
INTEGER  I 

C  FIND  Fl 

CALL  XPSYSP (X,F1) 

C  UPDATE  THE  STATE 
DO  400  1-1,6 

X(I)«X(I)+H*F1(I) 

400  CONTINUE 


END 
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SUBROUTINE  XPSYSE ( X , F , A, MOOT , T ) 

C  THIS  SUBROUTINE  EVALUATES  THE  FUNCTIONS  FOR  RK4SYSE 
C  (ORBITAL  DYNAMICS  FOR  TWO  BODY  PROBLEM  FOR  EVADER 
C  USING  THE  ROCKET  EQUATION: 

C  A«Ao/( l-MDOT*T) 

C  (X  VECTOR  «  [X  XDOT  Y  YDOT  Z  ZDOT]) 

C  (F  VECTOR  =  [XDOT  XDOUBLEDOT  YDOT  YDOUBLEDOT  ZDOT 
C  ZDOUBLEDOT  ADOT] ) 

REAL *8  X(6) ,F(8) ,RSQRD,CONST,MDOT,T,A, V 

RSQRD-X ( 1 ) *X ( 1 ) +X ( 3 ) *X ( 3 ) +X ( 5 ) *X ( 5 ) 

CONST—  3.9860 1 2E1 4/RSQRD/SQRT  ( RSQRD ) 

V«SQRT(  X(  2 ) *X( 2 )+X( 4 ) *X( 4 )  4*X(  6  )  *X(  6  )  ) 

F( 1 )«X( 2 ) 

F( 2 ) -CONST*X( 1 )+A*X( 2 )/V 
F(3)-X(4) 

F ( 4 )-CONST*X( 3 )+A*X( 4 )/V 
F(5)-X(6) 

F( 6 )»CONST*X( 5 )+A*X( 6 )/V 

F(7)-A*MDOT 

F(8)»MDOT*MDOT 

END 


SUBROUTINE  RK4SYSE( T,X,H,A,MDOT) 

C  THIS  SUBROUTINE  IS  A  ONE  STEP  RUNGE-KUTTA 
C  4TH  ORDER  INTEGRATOR  FOR  THE  EVADER  DYNAMICS 

REAL *8  T,X( 6 ) ,  H ,  Fl  (  8  ) ,  F2 ( 8  ) , F 3 ( 8 ) , F4 ( 8 ) 
REAL  * 8  H2,DUMX(6) , A , MDOT , AD , MDOTD 
INTEGER  I 

H2-0.5*H 

C  FIND  Fl 

CALL  XPSYSE(X,Fl,A,MDOT,T) 

C  FIND  F2 

DO  100  1-1,6 

DUMX( I )»X( I )+H2*Fl ( I ) 

100  CONTINUE 

AD-A+H2*F1 ( 7 ) 

MDOTD-MDOT+H2  *F1 ( 8 ) 

CALL  XPS YSE(DUHX,F2, AD, MDOTD, T) 
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C  FIND  F3 

DO  200  1*1,6 

DUMX{ I ) *X( I ) +H2*F2  ( I ) 

200  CONTINUE 

AD-A+H2*F2  ( 7 ) 

MDOTD-MDOT+H2*F2(  8) 

CALL  XPSYSE( DUMX, F3 ,AD,MDOTD,T ) 

C  FIND  F4 

DO  300  1-1,6 

DUMX( I )«X( I ) +H*F3 ( I ) 

300  CONTINUE 

AD«A+H*F3(7 ) 

MDOTD-MDOT+H2  *  F  3 ( 8 ) 

CALL  XPSYSE{ DUMX, F4 , AD,MDOTD,T ) 

C  UPDATE  THE  STATE 
DO  400  1-1,6 

X<I)«X(I)+H*(F1(I}+F2(I)+F2(I)+F3(I)+ 
+  F3(I)+F4(I) )/6.0 

400  CONTINUE 

A-A+H* ( Fl ( 7 ) +F2 (  7  )  +  F2  (  7  ) +F3 ( 7 )  + 

+  F3{ 7 )+F4( 7 ) )/6 . 0 

MDOT-MDOT+H* ( Fl ( 8 )+F2 ( 8 ) +F2 ( 8 ) +F3 ( 8  )  + 

+  F3( 8 )+F4 ( 8 ) )/6 . 0 

END 


SUBROUTINE  EULERE ( T , X , H , A , MDOT ) 

C  THIS  SUBROUTINE  IS  A  ONE  STEP  EULER  INTEGRATOR 
C  FOR  THE  EVADER  DYNAMICS 

REAL *8  T,X{6),F1(8),H,A, MDOT 
INTEGER  I 

C  FIND  Fl 

CALL  XPSYSE(X, Fl , A, MDOT, T) 

C  UPDATE  THE  STATE 
DO  400  1-1,6 

X(I)-X(I)+H*F1(I) 

400  CONTINUE 

A-A+H*F1 { 7 ) 

MDOT-MDOT+H *F1(  8) 


END 
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C  T00L2 

C  THIS  IS  A  COLLECTION  OF  SUBROUTINES  NEEDED  FOR 
C  COORDINATE  TRANSFORMATIONS 

C  IN  THE  HYPERVELOCITY  ORBITAL  INTERCEPT  PROGRAM 

C  SUBROUTINE  DICTIONARY 

XHAT  ESTIMATED  RELATIVE  STATE  VECTOR 

XP  STATE  VECTOR  OF  PURSUER 

XPD  DUMMY  STATE  VECTOR  OF  PURSUER 

TMAT  TRANSFORMATION  MATRIX 

TMATA  TRANSFORMATION  MATRIX 


SUBROUTINE  COMPTLOS( XHAT, TMAT) 

C  THIS  SUBROUTINE  COMPUTES  THE  MATRIX  (TMAT)  THAT 
C  TRANSFORMS  THE  REFERENCE  FRAME  TO  THE  LOS  FRAME 
C  WHERE  THE  X  AXIS  OF  THE  LOS  FRAME  LIES  ALONG  THE 
C  RELATIVE  POSTION  VECTOR. 

REAL *8  XHAT ( 6 ) , TMAT ( 3 , 3 ) , A , R , AR 

R-SQRT ' XHAT ( 1 ) *XHAT ( 1 ) +XHAT ( 3 ) *XHAT ( 3 ) + 

+  XHAT ( 5 ) *XHAT ( 5 ) ) 

A-SQRT ( XHAT ( X ) *XHAT ( 1 ) +XHAT ( 5 ) *XHAT ( 5 ) ) 

IF  (A  .LT.  .00001)  THEN  A«. 00001 
AR«A*R 

TMAT (1,1) *XHAT ( 1 ) /R 

TMAT (1,2) «XHAT ( 3 ) /R 

TMAT (1,3) «XHAT ( 5 ) /R 

TMAT ( 2 , 1 ) — XHAT( 1 ) *XHAT( 3 ) /AR 

TMAT ( 2,2 )-A/R 

TMAT( 2,3) «-XHAT( 3 ) *XHAT{ 5 )/AR 
TMAT(  3 , 1 )—  XHAT(  5  )/A 
TMAT( 3 , 2  )»0 . 0 
TMAT (3,3) «XHAT ( 1 ) /A 

END 


SUBROUTINE  COMPTV ( XP , TMAT ) 

C  THIS  SUBROUTINE  COMPUTES  THE  MATRIX  (TMAT)  THAT 
C  TRANSFORMS  THE  REFERENCE  FRAME  TO  THE  BODY  FRAME 
C  WHERE  THE  X  AXIS  OF  THE  BODY  FRAME  LIES  ALONG 
C  THE  PURSUER'S  VELOCITY  VECTOR  AND  THE  PURSUER'S 
C  RADIUS  VECTOR  IS  IN  THE  NEW  XY  PLANE. 
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REAL  *  8  XP( 6 ) , XPD ( 6 ) ,TMATA(3,3) ,TMAT(3,3) 

REAL *8  A ,  B ,  V ,  AV 

V«SQRT( XP ( 2 ) *XP ( 2 ) +XP ( 4 ) *XP ( 4 ) +XP ( 6 ) *XP ( 6 ) ) 
A-SQRT ( XP ( 2 ) *XP ( 2 ) +XP ( 6 ) *XP ( 6 ) ) 

IF  (A  .LT.  .00001)  THEN  A*. 00001 
AV«A*V 

TMAT (1,1) -XP ( 2 ) /V 

TMAT (1,2) -XP ( 4 ) /V 

TMAT (1,3) “XP ( 6 ) /V 

TMAT (  2 , 1 )— XP(  2  )  *XP  (  4  )/AV 

TMAT (2,2) -A/V 

TMAT( 2,3) -~XP( 4 ) *XP ( 6 )/AV 

TMAT(  3,1)— XP(6)/A 

TMAT{3,2)-0.0 

TMAT (3,3) -XP  {  2 )  /A 

CALL  TRANSFWD(XP(1) ,XP(3) ,XP( 5 ) ,TMAT,XPD( 1 ) , 

+  XPD ( 3 ) , XPD ( 5 ) ) 

B-SQRT(XPD( 3  j  *XPD( 3 )+XPD( 5 ) *XPD( 5 ) ) 

IF  (B  .LT.  .0001)  RETURN 

TMATA (  2 , 1 )  -  ( TMAT (2,1) *XPD ( 3 ) +TMAT (3,1) *XPD ( 5 ) ) /B 
TMATA (  3 , 1 )  -  ( TMAT (3,1) *XPD ( 3 ) -TMAT (2,1) *XPD ( 5 ) ) /B 
TMATA( 2 , 2 )-( TMAT( 2 , 2 ) *XPD( 3 )+TMAT( 3 , 2 ) *XPD( 5 ) )/B 
TMATA ( 3 , 2 ) - ( TMAT (3,2) *XPD ( 3 ) -TMAT (2,2) *XPD ( 5 ) ) /B 
TMATA ( 2 , 3 )-{ TMAT( 2,3) *XPD( 3 )+TMAT( 3,3) *XPD( 5 ) )/B 
TMATA ( 3 , 3 ) * ( TMAT (3,3) *XPD ( 3 ) -TMAT (2,3) *XPD ( 5 ) ) /B 
TMAT (2,1) -TMATA (2,1) 

TMAT (3,1) -TMATA (3,1) 

TMAT (2,2) -TMATA (2,2) 

TMAT (3,2) -TMATA (3,2) 

TMAT (2,3) -TMATA( 2,3) 

TMAT( 3 , 3 ) -TMATA ( 3,3) 

END 


SUBROUTINE  TRANS FWD ( X , Y , Z , TMAT , XT , YT , ZT ) 

C  THIS  ROUTINE  TAKES  THE  X,Y,Z  VECTOR  AND  USES 
C  THE  TRANSFORMATION  MATRIX  TMAT  TO  FORM  THE 
C  VECTOR  XT, YT,2T 

REAL*8  X , Y , Z , TMAT (3,3), XT , YT , ZT 
INTEGER  I 

XT-TKAT( 1 , 1 ) *X+TMAT( 1,2) *Y+TMAT( 1 , 3 ) *Z 
YT«TMAT( 2,1) *X+TMAT( 2,2) *Y+TMAT( 2 , 3 ) *Z 
ZT-TMAT( 3,1) *X+TMAT( 3 , 2 ) *Y+TMAT( 3 , 3 ) *Z 

END 
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SUBROUTINE  TRANSBKWD ( X ,  Y ,  Z  ,  TMAT , XT, YT, ZT) 

C  THIS  ROUTINE  TAKES  THE  X,Y,Z  VECTOR  AND  USES  THE 
C  INVERSE  OF  THE  TRANSFORMATION  MATRIX  TMAT  TO 
C  FORM  THE  VECTOR  XT,YT,ZT 

REAL *8  X , Y , Z , TMAT (3,3', XT , YT , ZT 
INTEGER  I 

XT-TMAT( 1 , 1 ) *X+TMAT( 2 , 1 ) *Y+TMAT( 3 , 1 ) *Z 
YT-TMAT (1,2) *X+TMAT( 2,2) *Y+TMAT ( 3 , 2 ) *Z 
ZT-TMAT ( 1 , 3 ) *X+TMAT ( 2 , 3 ) *Y+TMAT ( 3 , 3 ) *Z 


END 
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C  TOOL 3 

C  THIS  IS  A  COLLECTION  OF  SUBROUTINES  NEEDED  FOR 
C  EXTENDED  KALMAN  FILTERING 

C  IN  THE  HYPERVELOCITY  ORBITAL  INTERCEPT  PROGRAM 


C  SUBROUTINE  DICTIONARY 


c 

ACCDEN 

DENOMINATOR  OF  EVADER  ACCELERATION  TERM 

c 

ACCEL 

PRESENT  EVADER  THRUSTING  ACCELERATION 

c 

cov 

COVARIANCE  MATRIX 

c 

DET 

THE  DETERMINANT  OF  THE  MATRIX  'MAT' 

c 

DMAT 

DUMMY  MATRIX 

c 

DCMAT 

DUMMY  COLUMN  MATRIX 

c 

DT 

TIME  STEP 

r 

DUM 

DUMMY  VARIABLE 

C 

E 

RADIUS  OF  EVADER 

C 

E2 

E**2 

C 

E3 

E**3 

C 

E5 

E**5 

C 

EDOT 

VELOCITY  OF  EVADER 

C 

EDOT2 

EDOT* *2 

C 

EDOT3 

EDOT* *3 

C 

F 

MATRIX  OF  STATE  PARTIAL  DERIVATIVES 

c 

GAIN 

GAIN  MATRIX 

c 

GAMMA 

OBSERVED  LINE-OF-SIGHT  ANGLE  (IN  PLANE) 

c 

H 

MATRIX  OF  MEASUREMENT  PARTIALS 

c 

HYP13 

XHAT ( 1 ) *  *  2  +  XHAT ( 3 ) *  *  2 

c 

HYP15 

XHAT( 1 ) **2  +  XHAT ( 5 ) *  *  2 

c 

I 

COUNTER 

c 

J 

COUNTER 

c 

JUP 

UPPER  LIMIT  ON  'J'  COUNTER 

c 

K 

COUNTER 

c 

KFLAG 

GAIN  CONVERGENCE  FLAG 

c 

MEAN 

GAUSSIAN  MEAN 

c 

1  -  WITHOUT  FILTER 

c 

2  -  WITH  FILTER 

c 

3  -  WITH  FILTER  +  PRINTOUT 

r» 

w 

PDOT 

TIME  DERIVATIVE  OF  COVARIANCE  MATRIX 

c 

POLD 

PROPAGATED  COVARIANCE  MATRIX 

c 

Q 

VARIANCE  OF  FILTER  STATE  NOISE 

c 

R 

ESTIMATE  OF  RANGE  OF  EVADER  FROM  PURSUER 

c 

RES 

MEASUREMENT  RESIDUALS 

c 

R3 

VARIANCE  OF  MEASUREMENT  NO/SE 

c 

RANGE 

MEASUREMENT  OF  RELATIVE  RANGE 

c 

SEED 

SEED  FOR  RANDOM  NUMBER  GENERATOR 

c 

SIGMA 

GAUSSIAN  STANDARD  DEVIATION 

c 

SUM 

SUM  OF  UNIFORMLY  DISTRIBUTED  NUMBERS 

c 

T 

TIME 

c 

THETA 

OBSERVED  OUT-OF-PLANE  LOS  ANGLE 

c 

U 

GRAVITATIONAL  CONSTANT 

non  ooonooooooooooooo 
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UPDATE  FLAG  FOR  UPDATING  EKF 

0  NO  UPDATE 

1  UPDATE 

2  UPDATE  WITH  RESIDUALS  EQUAL  TO  ZERO 

V  ESTIMATE  OF  RELATIVE  RANGE  RATE 

VAR  VARIANCE  OF  MEASUREMENT 

XEEST  ESITIMATED  EVADER  VECTOR 

XHAT  ESTIMATE  OF  X  VECTOR  (BODY  FRAME) 

1  RELATIVE  X  POSITION 

2  RELATIVE  X  VELOCITY 

3  RELATIVE  Y  POSITION 

4  RELATIVE  Y  VELOCITY 

5  RELATIVE  Z  POSITION 

6  RELATIVE  Z  VELOCITY 

7  PRESENT  ACCELERATION  OF  EVADER 

8  BOOSTER  MASS  FLOW  RATE 

XPEST  ESTIMATED  PURSUER  VECTOR 


FUNCTION  GAUSS ( SEED, SW,G2) 

CREATES  A  POINT  HAVING  A  GAUSSIAN  DISTRIBUTION  WITH 
MEAN-0 . 0 
SIGMA-1.0 

REAL  MTH $ RANDOM, A, B 
REAL *8  GAUSS, G2 
INTEGER  SEED, SW, I 

IF  (SW  ,GT.  0)  THEN 
SW-0 

GAUSS-G2 

ELSE 

SW-1 

A»SQRT( -2 . Q*ALOG( MTH$RANDOM( SEED) ) ) 

B-2 . 0*ACOS ( -1 . 0 ) * ( MTH$RANDOM( SEED ) ) 

G2«A*COS(B) 

GAUSS«A*SIN(B) 

END  IF 

END 


SUBROUTINE  EKF8 ( XHAT , XEEST , XPEST , T , DT , COV ,Q,R3 , 

+  RANGE , THETA , GAMMA , KFLAG , RES , UPDATE ) 

C  THIS  SUBROUTINE  ESTIMATES  RELATIVE  POSITION  AND 
C  VELOCITY  VECTORS  AND  ACCELERATION  AND  MASS  FLOW 
C  RATE  OF  THE  EVADER  USING  AN  EIGHT  STATE  EXTENDED 
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C  KALMAN  FILTER  WITH  SERIAL  UPDATES  OF  RANGE  AND 
C  TWO  LINE-OF-SIGHT  ANGLES . 

C  ( GELB ,  'APPLIED  OPTIMAL  ESTIMATION' ,  PP,  182-192) 

REAL *8  XHAT ( 8 ) ,COV(8,8) , RANGE , THETA , GAMMA ,  F  (  8 , 8  ) 
REAL *8  E,E2,E3,E5,U, EDOT , EDOT2 , Q( 8 , 8  )  , RES ( 3 ) 

REAL *8  EDOT3 , T , HYP15 , HYPl 3  ,  H  (  8  ) 

REAL *8  XEEST ( 6 ) , XPEST( 6 ) , DT, PDOT( 8,8) ,R,R3(3,3) 
REAL* 8  POLD( 8,8) , DMAT , DCMAT ( 8 ) ,GAIN(8) 

INTEGER  I ,J,JUP,K, KFLAG, UPDATE 

C  INITIALIZE  DUMMY  VARIABLES 
U-3.986012E14 

E2-XEEST( 1 ) *XEEST ( 1 ) +XEEST( 3 ) *XESST( 3 ) + 

+  XEEST ( 5 ) *XEEST( 5 ) 

E«SQRT(E2) 

E3-E2+E 

E5-E3*E2 

EDOT2-XEEST( 2 ) *XEEST( 2 ) +XEEST( 4 ) *XEEST{ 4 )+ 

+  XEEST( 6 ) *XEEST( 6 ) 

EDOT-SQRT(EDOT2) 

EDOT3-EDOT*EDOT2 

C  COMPUTE  F  MATRIX 
DO  200  1-1,8 
F( 1 , I )-0 . 0 
F( 3 , I )»0 . 0 
F ( 5 , I )-0 . 0 
F( 7 , I ) -0 . 0 
F{ 8 , I )-0 . 0 
200  CONTINUE 

F(l,2)-1.0 
F( 3 , 4 ) -1 . 0 
F( 5 , 6 ) -1 . 0 

F( 2 , 1 ) - ( -1 , 0+3 . 0*XEEST( 1 ) *XEEST( 1 )/E2 ) *U/E3 

F( 2,2 )-( 1 . 0-XEEST( 2 ) *XEEST( 2 )/EDOT2 ) *XHAT( 7 )/EDOT 

F( 2 , 3 ) -3 . 0*U*XEEST( 1 ) *XEEST( 3 )/E5 

F(  2 , 4  )—  XEEST(  2  )  *XHAT(  7  )  *XEEST(  4  )/EDOT3 

F ( 2 , 5 ) -3 . 0*U*XEEST( 1 ) *XEEST( 5 )/E5 

F ( 2 , 6 ) — XEEST{ 2 ) *XHAT( 7 ) *XEEST( 6 ) /EDOT 3 

F{ 2  » 7 ) «XEEST( 2 )/EDOT 

F( 2,8)-0 .0 

F( 4 , 1 )-F( 2 , 3 ) 

F( 4 , 2 ) -F{ 2 , 4 ) 

F( 4 , 3 )-{ -1 . 0+3 . 0*XEEST( 3 ) *XEEST( 3 )/E2 ) *U/E3 

F( 4 , 4 )-( 1 . 0-XEEST( 4 ) *XEEST( 4 )/EDOT2 ) *XHAT( 7 )/EDOT 

F( 4 , 5 ) -3 . 0*U*XEEST( 3 ) * XEEST ( 5 )/E5 

F  (  4 , 6  )  —XEEST  (  4  )  *XHAT ( 7  )  *XEEST (  6  )  /EDOT 3 

F(  4 ,7  )<=XEEST(  4  )/EDOT 

F( 4 , 8  )«0 . 0 


na  LntlSu  wv  yuuv  JV  L!<uMiliniUVW>iUV'iniJtVVMUVUVitfKV>(1.VU1 


D-29 


F( 6 , 1 )-F( 2 , 5 ) 

F(6,2)-F(2,6) 

F( 6 , 3 )»F( 4 , 5 ) 

F(6,4)-F(4,6) 

F(6,5)-(-1.0+3.0*XEEST(  5)*XEEST( 5)/E2)*U/E3 
F ( 6 , 6 ) - ( 1 . 0-XEEST ( 6 ) *XEEST ( 6 ) /EDOT2 ) *XHAT ( 7 ) /EDOT 
F { 6 , 7 ) =XEEST ( 6 ) /EDOT 
F( 6 , 8  )-0 . 0 

F( 7 , 7 )*XHAT( 8 ) 

F  (  7 , 8  )  -XHAT  {  7  ) 

F  (  8 , 8  )  -XHAT  (  8  )  +XHAT  (  8  ) 

C  PROPAGATE  COVARIANCE  MATRIX  FORWARD  (EULER'S  METHOD) 
C  (USING  SYMMETRY,  COMPUTE  LOWER  TRIANGULAR  PDOT) 

DO  300  1-1,8 
DO  300  J-1,I 

PDOT ( I , J ) -Q ( I , J ) 

DO  300  K-l , 8 

PDOT ( I , J ) -PDOT  (I,J)+F(I,K) *COV ( K , J )  + 

+  COV (I,K)*F(J,K) 

300  CONTINUE 

DO  310  1-1,8 
DO  310  J-1,I 

POLD ( I , J ) -COV ( I , J ) + ( PDOT ( I , J ) *DT ) 

310  CONTINUE 

C  REASSIGN  COV  AND  ZERO  OUT  H  MATRIX 
DO  320  1-1,8 

COV (1,1) -POLD (1,1) 

H(I)»0.0 

JUP-I-1 

DO  320  J-1,JUP 

COV( I , J )-POLD( I , J) 

COV ( J , I ) -POLD ( I , J ) 

320  CONTINUE 

C  PROPAGATE  STATE  ESTIMATE  FORWARD  ONE  STEP 

CALL  RK4SYSE( T, XFEST, DT,XHAT(  7  )  ,XHAT(  8  )  ) 

IF  (UPDATE  .NE.  1)  CALL  RK4SYSP(T,XPEST,DT) 

DO  330  1-1,6 

XHAT( I )-XEEST( I )-XPEST( I ) 

330  CONTINUE 

IF  (UPDATE  .NE.  0)  THEN 

C  PERFORM  RANGE  UPDATE 

R-SQRT ( XHAT ( 1 ) *XHAT ( 1 ) +XHAT ( 3 ) *XHAT ( 3 ) + 

+  XHAT( 5 ) *XBAT( 5 ) ) 

H ( 1 ) «XHAT( 1 )/R 
H( 3 ) -XHAT( 3 )/R 
H{ 5) -XHAT ( 5)/R 


mm® 
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RES ( 1 ) -RANGE-R 

IF  (UPDATE  .NB.  1)  RES(l)-0.0 

CALL  UPDATE8 ( XHAT , H , COV , R3 ( 1 , 1 ) , RES ( 1 ) , KFLAG ) 

C  PERFORM  THETA  UPDATE 

HYP1 5-XHAT ( 1 ) *XHAT ( 1 ) +XHAT ( 5 ) *XHAT ( 5 ) 

H ( 1 ) --XHAT ( 5 ) /HYPl 5 
H(3)-0.0 

H( 5) -XHAT ( 1) /HYPl 5 

RES  (  2  )  -THETA-ATAN  (  XHAT  (  5  )  /XHAT ( 1 )  ) 

IF  (UPDATE  .NE.  1)  RES(2)»0.0 

CALL  UPDATES ( XHAT, H, COV, R3( 2, 2) ,RES(2) , KFLAG) 

C  PERFORM  GAMMA  UPDATE 

HYPl 3-XHAT ( 1 ) *XHAT ( 1 ) +XHAT ( 3 ) *XHAT ( 3 ) 

H ( 1 ) «-XHAT ( 3 ) /HYPl 3 
H ( 3 ) “XHAT ( 1 ) /HYPl 3 
H(5)-0.0 

RES ( 3 ) -GAMMA- ATAN ( XHAT ( 3 ) /XHAT ( 1 ) ) 

IF  (UPDATE  .NE.  1)  RES(3)-0.0 

CALL  UPDATES ( XHAT , H , COV , R3 ( 3 , 3 ) , RES ( 3 ) , KFLAG ) 

END  IF 

END 


SUBROUTINE  UPDATE 8 ( XHAT , H , COV , VAR , RES , KFLAG ) 

C  THIS  SUBROUTINE  DOES  ONE  SERIAL  UPDATE  FOR  THE 
C  EIGHT  STATE  EXTENDED  KALMAN  FILTER 

REAL *8  XHAT ( 8 ) ,H( 8 ) , COV( 8 , 8 ) , VAR, RES 
REAL* 8  DMAT, DCMAT{ 8 ) ,GAIN(8) ,POLD(8,8) 
INTEGER  I , J ,JUP, KFLAG 

C  INITIALIZE  COUNT 
COUNT- 0 

C  COMPUTE  MATRIX  (1X1)  FOR  INVERSION 
DO  110  1-1,8 
DCMAT( I ) -0 . 0 
DO  110  J-1,8 

DCMAT  ( I ) -DCMAT ( I  UCOV (  X  ,  J )  *H  ( J ) 

110  CONTINUE 
DMAT-VAR 
DO  120  1-1,8 

DMAT-DMAT+DCMAT( I ) *H( I ) 

120  CONTINUE 
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C  COMPUTE  GAIN  MATRIX 
DO  140  1-1,8 
GAIN( I )«0 . 0 
DO  130  J«1 , 8 

GAIN(I)-GAIN(I)+COV(I,J)*H(J) 

130  CONTINUE 

GAIN( I )-GAIN( I )/DMAT 
140  CONTINUE 

C  UPDATE  COVARIANCE  MATRIX  AND  LIMIT  THE  GAIN  TO 
C  PREVENT  NEGATIVE  DIAGONAL  COVARIANCES 
DO  150  1-1,8 
DCMAT( I ) -0 . 0 
DO  150  J-1,8 

DCMAT ( I ) -DCMAT ( I ) +H ( J ) *  COV ( J , I ) 

150  CONTINUE 

DO  160  1-1,8 

POLD ( I , I )«COV( I , I )-GAIN( I ) *DCMAT( I ) 

IF  ( POLD( 1,1)  .LT.  0.0)  THEN 
IF  (COUNT  .LT.  10)  THEN 
DO  155  J-1,8 

GAIN ( J ) -GAIN ( J ) /2 . 0 
155  CONTINUE 

COUNT-COUNT+1 
KFLAG-KFLAG+1 
GOTO  150 
ELSE 

KFLAG-KFLAG+990 
RETURN 
END  IF 
END  IF 
JUP-I-1 

DO  160  J-1,JUP 

POLD ( I , J ) -COV( I , J ) -GAIN ( I ) *DCMAT ( J ) 

160  CONTINUE 

C  UPDATE  ESTIMATE 
DO  170  1-1,8 

XHAT{ I ) -XHAT{ I )+GAIN( I ) *RES 
170  CONTINUE 

C  UPDATE  COV  MATRIX 
DO  175  1-1,8 

COV (1,1) -POLD (1,1) 

JUP-I-1 

DO  175  J-1,JUP 

COV ( I , J ) -POLD ( I , J ) 

COV ( J , I ) -POLD ( I ,  J ) 

175  CONTINUE 


END 
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SUBROUTINE  EKF6  (  XHAT ,  XEEST ,  XPEST , T , DT ,  COV ,  0  ,  R3  , 

+  RANGE , THETA , GAMMA , KFLAG , RES , UPDATE ) 

C  THIS  SUBROUTINE  ESTIMATES  RELATIVE  POSITION  AND 
C  VELOCITY  VECTORS  AND  ACCELERATION  OF  THE  EVADER 
C  USING  A  SIX  STATE  EXTENDED  KALMAN  FILTER  WITH 
C  SERIAL  UPDATES  OF  RANGE  AND  TWO  LOS  ANGLES. 

C  (GELB,  'APPLIED  OPTIMAL  ESTIMATION' ,  PP.  182-192) 

REAL  *  8  XHAT  ( 8 ) ,  COV  (8,8)  ,  RANGE ,  THETA  ,  GAMMA ,  r!  { 8 , 8 ) 
REAL *8  E,E2,E3,E5,U,EDOT,EDOT2,Q(8,8) ,RES(3) 

REAL *8  EDOT3 , T , HYPl 5 , HYP13 ,  H(  8  ) 

REAL*8  XEEST(  6  )  ,XPEST(6)  ,  DT ,  PDOT  ( 8  ,  S  )  ,R,P.3(3,3) 
REAL *8  POLD(8,8) ,  DMAT , DCMAT ( 8 ) ,GAIN(8) 

INTEGER  I, J,JUP,K, KFLAG, UPDATE 

C  INITIALIZE  DUMMY  VARIABLES 
U-3.986012E14 

E2-XEEST( 1 ) *XEEST( 1 )+XEEST( 3 ) *XEEST( 3 )+ 

+  XEEST( 5 ) *XEEST( 5 ) 

E-SQRT(E2) 

E3«E2*E 

E5-E3*E2 

EDOT2-XEEST( 2 ) *XEEST( 2 ) +XEEST( 4 ) *XEEST( 4  )  + 

+  XEEST( 6 ) *XEEST( 6 ) 

EDOT-SQRT( EDOT2 ) 

EDOT3-EDOT*EDOT2 

C  COMPUTE  F  MATRIX 
DO  200  1-1,8 
F{  1 , 1 ) -0 . 0 
F(  3 , 1 )-0 . 0 
F(  5,1 )-0 .0 
F(  7 , 1 )-0 . 0 
F(  8 , 1 )-0 .0 
200  CONTINUE 

F( 1 , 2 )-l .  0 
F( 3 , 4  )-l . 0 
F( 5, 6 )-l . 0 

F ( 2 , 1 ) - { -  1 . 0+3 . 0*XEEST( 1 J *XEEST( 1 )/E2 ) *U/E3 

F{ 2 , 2 )* ( 1 . 0-XEESTt 2 ) *XBEST( 2  j /EDOT2 ) *XHAT( 7 )/EDOT 

F ( 2 , 3 ) -3 . 0*U*XEEST( 1 ) *XEEST( 3 ) /E5 

F( 2 , 4 )-  -XEEST ( 2 ) *KHAT( 7 ) *XEEST( 4 )/EDOT3 

F{ 2 , 5 ) -3 . 0*U*XEEST( 1 ) *XEEST( 5 )/E5 

F ( 2 , 6 ) — XEEST( 2 ) *XHAT{ 7 ) *XSEST( 6 )/EDOT3 

F ( 2 , 7 ) -0 . 0 

F(2,3)-0.0 


F( 4 , 1 )-F( 2 , 3 ) 

F{ 4 , 2 )-F( 2 , 4 ) 

F( 4 , 3 ) - ( -1 . 0+3 . 0*XEEST( 3 ) *XEEST< 3 )/E2 ) *U/E3 
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F  (  4 , 4  )  -  ( 1 . O-XEEST ( 4 ) *XEEST ( 4 ) /ED0T2 ) *XHAT ( 7 ) /EDOT 
F(4,5)-3.0*U*XEEST( 3 ) *XEEST( 5 )/E5 
F { 4 , 6 ) — XEEST ( 4 ) *XHAT ( 7 ) *XEEST ( 6 ) /EDOT3 
F( 4 , 7 ) *0 . 0 
F( 4 ,8 )-0 .0 

F(6,1)«F(2,5) 

F( 6 , 2 ) »F( 2 , 6 ) 

F(6,3)-F(4,5) 

F(6#4)-F(4#6) 

F( 6 , 5 )-( -1 . 0+3 . 0*XEEST( 5 ) *XEEST( 5 )/E2 ) *U/E3 
F  (  6 , 6  ) « ( 1 . 0-XEEST (  6  )  *XEEST (  6  )  ./EDOT2  )  *XHAT  ( 7  )  /EDOT 
F(6,7)-0.0 
F( 6 ,8 )«0 .0 

C  PROPAGATE  COVARIANCE  MATRIX  FORWARD  (EULER'S  METHOD) 
C  (USING  SYMMETRY,  COMPUTE  LOWER  TRIANGULAR  PDOT) 

DO  300  1-1,6 
DO  300  J«1,I 

PDOT ( I » J ) -Q ( I , J ) 

DO  300  K«1 , 6 

PDOT( I , J )-PDOT( I , J ) +F ( I , K ) *COV( K , J ) + 

+  COV( I ,  K) *F( J , K ) 

300  CONTINUE 

DO  310  1-1,6 
DO  310  J-1,I 

POLD(I,J)«COV(I,J)+(PDOT(I,J)*DT) 

310  CONTINUE 

C  REASSIGN  COV  AND  ZERO  OUT  H  MATRIX 
DO  320  1-1,6 

COV (1,1) -POLD (1,1) 

H(I )-0,0 
JUP-I-1 

DO  320  J-1,JUP 

COV ( I , J ) -FOLD ( I , J ) 

COV( J,I)«POLD(I,J) 

320  CONTINUE 

C  PROPAGATE  STATE  ESTIMATE  FORWARD  ONE  STEP 

CALL  RK4SYSE( T, XEEST, DT,XHAT( 7 ) ,XHAT( 8 ) ) 

IF  (UPDATE  .NE.  1)  CALL  RK4SYSP(T,XPEST,DT) 

DO  330  1-1,6 

XHAT( I )-XEEST( I )-XPEST( I ) 

330  CONTINUE 

IF  (UPDATE  .NE.  0)  THEN 

C  PERFORM  RANGE  UPDATE 

R-SQRT(XHAT( 1 ) *XHAT( 1 )+XHAT( 3 ) *XHAT{ 3 )+ 

+  XHAT ( 5 ) *XHAT ( 5 ) ) 

H( 1 )«XHAT( 1 )/R 
H{ 3 ) -XHAT{ 3 )/R 
H( 5 )-XHAT( 5 )/R 


RES ( 1 ) “RANGE-R 

IF  (UPDATE  .NE.  1)  RES(l)-0.0 

CALL  UPDATE6 ( XHAT , H , COV , R3 ( 1 , 1 ) ,RES(1) , KFLAG ) 


D-34 


C  PERFORM  THETA  UPDATE 

HYPl 5-XHAT ( 1 ) *XHAT ( 1 ) +XHAT ( 5 ) *XHAT ( 5 ) 

H{1)—  XHAT(5)/HYP15 
H(3)-0.0 

H ( 5 ) “XHAT ( 1 ) /HYP 1 5 

RES ( 2 ) * THETA- ATAN ( XHAT ( 5 } /XHAT ( 1 ) ) 

IF  (UPDATE  .NE.  1)  RES(2)«0.0 

CALL  UPDATE6 ( XHAT, H , COV , R3 (2,2) ,RES(2) , KFLAG) 

C  PERFORM  GAMMA  UPDATE 

HYP13«XHAT( 1 ) *XHAT( 1 )+XHAT( 3 ) *XHAT( 3 ) 

H(  1 )— XHAT(  3  ) /HYPl 3 
H ( 3 ) “XHAT ( 1 ) /HYP 1 3 
H( 5)-0.0 

RES ( 3 ) “GAMMA- ATAN ( XHAT ( 3 ) /XHAT ( 1 ) ) 

IF  (UPDATE  .NE.  1)  RES(3)“0,0 

CALL  UPDATE 6 ( XHAT, H, COV,R3{ 3,3) ,RES( 3) , KFLAG) 

END  IF 

END 


SUBROUTINE  EKF6Q ( XHAT , XEEST , XPEST , T , DT , COV , Q , R3 , 
+  RANGE, THETA, GAMMA, KFLAG, RES) 

C  THIS  SUBROUTINE  ESTIMATES  RELATIVE  POSITION  AND 
C  VELOCITY  VECTORS  AND  ACCELERATION  OF  THE  EVADER 
C  USING  A  SIX  STATE  EXTENDED  KALMAN  FILTER  WITH 
C  SERIAL  UPDATES  OF  RANGE  AND  TWO  LOS  ANGLES. 

C  (GELB,  'APPLIED  OPTIMAL  ESTIMATION',  PP.  182-192) 

REAL *8  XHAT (8 ) ,COV( 8,8) , RANGE , THETA, GAMMA , F( 8,8) 
REAL* 8  E,E2,E3,E5,U, EDOT , EDOT2 , Q( 8 , 8 ) , RES( 3 ) 

REAL *8  EDOT 3 , T, HYP15 , HYPl 3 , H( 8 ) 

REAL *6  XEEST ( 6 ) , XPEST ( 6 ) , DT , PDOT( 8,8) ,R,R3(3,3) 
REAL *8  FOLD ( 8 , 8 ) , DMAT , DCMAT ( 8 ) , GAIN ( 8 ) 

INTEGER  I, J,JUP,K, KFLAG 

C  INITIALIZE  DUMMY  VARIABLES 
U“3 . 986012E14 

E2-XEEST( 1 ) *XEEST( 1 ) +XEEST( 3 ) *XEEST( 3 ) + 

+  XEEST( 5 ) *XEEST( 5 ) 

E“SQRT(E2) 

E3“E2*E 

E5«E3*E2 

EDOT2“XEEST( 2 ) *XEEST{ 2 ) +XEEST( 4 ) *XEEST( 4 )  + 

+  XEEST{ 6 ) *XEEST( 6 ) 


urauBtrau*  unuawiiu* 
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EDOT-SQRT ( EDOT2 ) 

EDOT3«EDOT*EDOT2 

C  COMPUTE  F  MATRIX 
DO  200  1-1,8 
DO  200  J-1,8 
F ( I , J ) -0 . 0 
200  CONTINUE 

F( 1 , 2 ) -1 . 0 
F( 3 , 4 ) -1 . 0 
F( 5 , 6 )»1 . 0 

F ( 2 , 2 ) « ( 1 . 0-XEEST ( 2 ) *XEEST ( 2 ) /EDOT2 ) *XHAT ( 7 ) /EDOT 
F(  2 , 4  )  —  XEEST{  2  )  *XHAT(  7  )  *XEEST(  4  )/EDOT3 
F ( 2 , 6 ) — XEEST( 2 ) *XHAT( 7 ) *XEEST( 6 )/EDOT3 

F (  4  2 ) «F{  2  4  ) 

F ( 4 \ 4 ) - ( 1 . 0-XEEST ( 4 ) *XEEST ( 4 ) /EDOT2 ) *XHAT ( 7 ) /EDOT 
F ( 4 , 6 ) — XEEST( 4 ) *XHAT( 7 ) *XEEST( 6 )/EDOT3 

F( 6 ,2 )«F( 2,6 ) 

F(6,4)-F(4,6) 

F ( 6 , 6 ) - ( 1 . 0-XEEST ( 6 ) *XEEST ( 6 ) /EDOT2 ) *XHAT ( 7 ) /EDOT 

C  PROPAGATE  COVARIANCE  MATRIX  FORWARD  (EULER'S  METHOD) 
C  (USING  SYMMETRY,  COMPUTE  LOWER  TRIANGULAR  PDOT) 

DO  300  1-1,6 
DO  300  J-1,I 

PDOT ( I , J ) -Q ( I , J ) 

DO  300  K-1,6 

PDOT( I , J )-PDOT( I , J )+F( I , K ) *COV( K, J )+ 

+  COV{ I , K) *F( J , K) 

300  CONTINUE 

DO  310  1-1,6 
DO  310  J-1,I 

POLD( I , J )-COV ( I , J )+( PDOT( I , J ) *DT) 

310  CONTINUE 

C  REASSIGN  COV  AND  ZERO  OUT  H  MATRIX 
DO  320  1-1,6 

COV( 1,1) -POLD( 1,1) 

H( I )-0. 0 
JUP-I-1 

DO  320  J-1,JUP 

COV( I , J ) -POLD( I , J ) 

COV ( J , I ) -POLD ( I , J ) 

320  CONTINUE 

C  PROPAGATE  STATE  ESTIMATES  FORWARD  ONE  STEP 
XEEST( 1 ) -XEEST( 1 ) +DT*XEEST( 2 ) 

XEEST( 3 ) -XEEST ( 3 ) +DT*XEEST( 4 ) 

XEEST(  5  )-XEEST(  5 } -»-DT*XEEST{  6  ) 
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XEEST( 2 ) »XEEST ( 2 ) * ( 1 . 0+DT*XHAT ( 7 ) /FOOT ) 
XEEST(  4  )  -XEEST (  4  )  *  ( 1 . 0+DT*XHAT(  7  )  /EDOT ) 
XEEST( 6 ) =XEEST( 6 ) * ( 1 . 0+DT*XHAT ( 7 )/EDOT ) 
XHaT { 7 ) =XHAT ( 7 ) / ( 1 . 0-XHAT ( 8 ) *DT ) 

XHAT( 8 )-XHAT{ 8 )/( 1 . 0-XHAT( 8 ) *DT) 

DO  330  1-1,6 

XHAT ( I ) =XEEST ( I ) -XPEST ( I ) 

330  CONTINUE 


C  PERFORM  RANGE  UPDATE 

R-SQRT ( XHAT ( 1 ) *XHAT { 1 ) +XHAT ( 3 ) *XHAT ( 3 ) + 

+  XHAT ( 5 ) *XHAT ( 5 ) ) 

H ( 1 ) -XHAT ( 1 ) /R 
H(3)-XHAT(3)/R 
H( 5) -XHAT ( 5)/R 
RES ( 1 ) -RANGE-R 

CALL  UPDATE6 ( XHAT , H , COV , R3 ( 1 , 1 ) , RES ( i ) , KFLAG ) 

C  PERFORM  THETA  UPDATE 

HYP 1 5-XHAT ( 1 ) *XHAT ( 1 ) +XHAT ( 5 ) *XHAT ( 5 ) 

H ( 1 ) --XHAT ( 5 ) /HYPl 5 
H{ 3  )-0 . 0 

H ( 5 ) -XHAT ( 1 ) /HYPl 5 

RES ( 2 ) -TKETA-ATAN { XHAT ( 5 ) /XHAT ( 1 ) ) 

CALL  UPDATE6 ( XHAT , H , COV , R3 ( 2 , 2 ) , RES ( 2 ) , KFLAG) 

C  PERFORM  GAMMA  UPDATE 

HYP 1 3 -XHAT ( 1 ) *  XHAT ( 1 ) +XHAT ( 3 ) *XHAT ( 3 ) 

H{  1 )— XHAT(  3 ) /HYPl 3 
H{ 3 )-XHAT( 1 ) /HYPl 3 
H(5)«0.0 

RES ( 3 ) -GAMMA-ATAN { XHAT ( 3 ) /XHAT {1 ) ) 

CALL  UPDATE6 ( XHAT , H , COV , R3 ( 3 , 3 ) , RES { 3 ) , KFLAG) 


END 


SUBROUTINE  UPDATE6 ( XHAT , H , COV, VAR , RES , KFLAG ) 

C  THIS  SUBROUTINE  DOES  ONE  SERIAL  UPDATE  FOR  THE 
C  SIX  STATE  EXTENDED  KALMAN  FILTER 

REAL* 8  XHAT ( 8 ) , H ( 8 ) , COV (8,8), VAR , RES 
REAL *8  DMAT, DCMAT( 8 ) , GAIN ( 8 ) , POLD( 8,8) 
INTEGER  I, J,JUP, KFLAG 

C  INITIALIZE  COUNT 
COUNT-O 


C  COMPUTE  MATRIX  ( 1X1 )  FOR  INVERSION 
DO  110  1=1,6 
DCMAT( I ) =0 . 0 
DO  110  J=1 , 6 

DCMAT(I)-DCMAT(I)+COV(I,J)*H( J) 

110  CONTINUE 
DMAT-VAR 
DO  120  1=1,6 

DMAT»DMAT+DCMAT(  I )  *H(  I  ) 

120  CONTINUE 

C  COMPUTE  GAIN  MATRIX 
DO  140  1=1,6 
GAIN(  I  )«*0 . 0 
DO  130  J=1 , 6 

GAIN( I )«GAIN( I )+COV( I , J ) *H( J ) 

130  CONTINUE 

GAIN( I )-GAIN{ I )/DKAT 
140  CONTINUE 

C  UPDATE  COVARIANCE  MATRIX  AND  LIMIT  THE  GAIN  TO 
C  PREVENT  NEGATIVE  DIAGONAL  COVARIANCES 
DO  150  1=1,6 
DCMAT ( I ) «0 . 0 
DO  150  J«1 , 6 

DCMAT ( I ) -DCMAT ( I ) +H ( J ) * COV ( J , I ) 

150  CONTINUE 

DO  160  1=1,6 

POLD{ 1,1) »COV { I , I ) -GAIN{ I ) * DCMAT ( I ) 

IF  ( POLD( 1,1)  .LT.  0.0)  THEN 
IF  (COUNT  .LT.  10)  THEN 
DO  155  J-1,6 

GAIN ( J ) -GAIN ( J )/2 . 0 
155  CONTINUE 

COUNT- COUNT+1 
KFLAG-KFLAG+l 
GOTO  150 
ELSE 

KFLAG-KFLAG+990 
RETURN 
ENDIF 
END  IF 
JUP-I-1 

DO  160  J«1,JUP 

P0LD( I , J ) =COV( I , J )-GAIN{ I ) * DCMAT ( J ) 

160  CONTINUE 

C  UPDATE  ESTIMATE 
DO  170  1-1,6 

XHAT ( I ) -XHAT ( I ) +GAI N ( I )*RES 
170  CONTINUE 
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C  UPDATE  COV  MATRIX 
DO  175  1-1,6 

COV(I,I )-POLD(I,I) 
JUP-I-1 

DO  175  J-1,JUP 

COV ( I ,  J ) -POLD ( I ,  J  ) 
COV(J,I)-POLD(I,J) 
175  CONTINUE 

END 


SUBROUTINE  EKF3 ( XHAT , XEEST , XPEST , T , DT , COV , Q , R3 , 

+  RANGE, THETA, GAMMA, KFLAG, RES) 

C  THIS  SUBROUTINE  ESTIMATES  RELATIVE  POSITION  AND 
C  VELOCITY  VECTORS  AND  ACCELERATION  OF  THE  EVADER 
C  USING  A  THREE  STATE  EXTENDED  KALMAN  FILTER  WITH 
C  SERIAL  UPDATES  OF  RANGE  AND  TWO  LOS  ANGLES. 

C  (GELB,  'APPLIED  OPTIMAL  ESTIMATION',  PP.  182-192) 

REAL *8  XHAT (  8  )  ,  COV (8,8)  ,  RANGE ,  THETA,  GAMMA,  F{  8  ,  'i  ) 
REAL *8  E , E2 , E3 , E5 , U , EDOT , EDOT2 , Q( 8 , 8 ) , RES( 3 ) 
REAL* 8  EDOT3 , T , HYPl 5 , HYPl 3 , H ( 8 ) 

REAL *8  XEEST ( 6 ) , XPEST ( 6 ) , DT , PDOT (8,8),R,R3(3,3) 
REAL *8  PGLD( 8,8 ) , DMAT,DCMAT( 8 ) ,GAIN( 8 ) 

INTEGER  I , J,JUP,K, KFLAG 

C  INITIALIZE  DUMMY  VARIABLES 
U-3.986012E14 

E2-XEESTC 1 ) *XEBST( 1 ) +XEEST( 3 ) *XEEST{ 3 ) + 

+  XEEST( 5 )*XEEST{ 5) 

E-SQRT{ E2 ) 

E3-E2*E 

E5-E3*E2 

EDOT2-XEEST( 2 ) *XEEST{ 2 ) +XEEST{ 4 ) *XEEST( 4 )  + 

+  XE5ST(  6  )  *XEEST(  6  ) 

EDOT«SQRT( EDOT2 ) 

EDOT  3 -EDOT  *  EDOT2 

C  COMPUTE  F  MATRIX 
DO  200  1-1,8 
DO  200  J-1,8 
F( I , J)»0.0 
200  CONTINUE 

F{1,1)-DT *(-1,0+3. 0* XEEST { 1 ) *XEEST( 1 )/E2 ) *U/E3 
F( 1 , 3 ) -DT*  3 . 0*U*XEEST( 1 ) *XEEST{ 3 )/E5 
F{ 1 , 5 )-DT*3 . 0*U*XE£ST( 1 ) *XEEST( 5 )/E5 
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F(3,l)-F(l,3) 

F( 3 , 3 ) -DT* (-1.0+3 . 0*XEEST( 3 ) *XEEST( 3 )/E2 ) *U/E3 
F(  3 , 5 )-DT*3 . 0*U*XEEST( 3 ) *XEEST( 5 )/E5 

F( 5,1 )-F(l , 5) 

F(5,3)-F(3,5) 

F(  5 , 5  )  -DT*  (-1.0+3 . 0*XEEST (  5  )  *XFEST(  5  )/E2  )  *U/E3 

C  PROPAGATE  COVARIANCE  MATRIX  FORWARD  (EULER'S  METHOD) 
C  (USING  SYMMETRY,  COMPUTE  LOWER  TRIANGULAR  PDOT) 

DO  300  1-1 , 5 , 2 
DO  300  J-l ,  I  ,  2 
PDOT(I, J)-Q(I,J) 

DO  300  K=1 ,5,2 

PDOT ( I , J ) -PDOT ( T , J )+F( I ,K) *COV (K, J)+ 

+  COV (I,K)*F(J,K) 

300  CONTINUE 

DO  310  1-1,5, 2 
DO  310  J-1,1,2 

POLD( I , J)»COV( I , J )+( PDOT( I , J ) *DT) 

310  CONTINUE 

C  REASSIGN  COV  AND  ZERO  OUT  H  MATRIX 
DO  320  1-1,5, 2 

CQV( I , I )«POLD( 1,1) 

H( I  )«0 . 0 
H( 1+1 )»0 . 0 
JUP-I-2 

DO  320  J-3. ,  JUP,2 
COV ( I , J ) -POLD ( I , J ) 

COV( J,I)«POLD(I ,J) 

320  CONTINUE 

C  PROPAGATE  STATE  ESTIMATES  FORWARD  ONE  STEP 
CALL  RK4SYSE(T,XEEST, DT,XUAT( 7 ) ,XHAT( 8 ) ) 

DO  330  1-1,6 

XHAT( I ) -XEEST( I )-XPEST( I ) 

330  CONTINUE 

C  PERFORM  RANGE  UPDATE 

R-SQRT ( XHAT ( 1 ) *XHAT { 1 ) +XHAT ( 3 ) *XHAT { 3 ) + 

+  XHAT( 5 ) *XHAT( 5 ) ) 

H(1)*XHAT(1)/R 
H( 3 )-XHAT( 3 )/R 
H( 5 )*XHAT( 5 )/R 
RES ( 1 ) -RANGE-R 

CALL  UPDATE 3 ( XHAT , H , COV , R3 ( 1 , 1 ) , RES ( 1 ) , KFLAG ) 

C  PERFORM  THETA  UPDATE 

HYP15-XHAT{ 1 ) *XHAT( 1 ) +XHAT{ 5 ) *XHAT( 5 ) 

H  ( 1 )  —XHAT  (  5  j  /H YP1 5 
H( 3 )«0 . 0 

H ( 5 ) -XHAT ( 1 ) /HYP 1 5 
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RES ( 2 ) -THETA-ATAN ( XHAT ( 5 ) /XHAT ( 1 ) ) 

CALL  UPDATE3 ( XHAT, H , COV, R3 (2,2) ,RES(2) , KFLAG ) 

C  PERFORM  GAMMA  UPDATE 

HYP 1 3 =XH AT ( 1 ) *  XHAT ( 1 ) +XHAT ( 3 ) *XHAT ( 3 ) 

H{ 1 )=-XHAT( 3 )/HYPl3 
H ( 3 ) =XHAT ( 1 ) /HYP 1 3 
H( 5 )  =  0 . 0 

RES ( 3 ) =GAMMA-ATAN ( XHAT ( 3 ) /XHAT ( 1 ) ) 

CALL  UPDATE3(XHAT,H,COV,R3( 3,3) ,RES( 3) , KFLAG) 


END 


SUBROUTINE  UPDATE 3 ( XHAT , H , COV , VAR , RES , KFLAG ) 

C  THIS  SUBROUTINE  DOES  ONE  SERIAL  UPDATE  FOR  THE 
C  THREE  STATE  EXTENDED  KALMAN  FILTER 

REAL *8  XHAT ( 8 ) ,H{8) ,COV(8,8) , VAR, RES 
REAL  *  8  DMAT , DCMAT ( 8 ) ,GAIN(8) ,POLD(8,8) 
INTEGER  I, J, JUP, KFLAG 

C  INITIALIZE  COUNT 
COUNT-0 

C  COMPUTE  MATRIX  (lXl)  FOR  INVERSION 
DO  110  1-1,5, 2 
DCMAT ( I ) -0 . 0 
DO  110  J-1,5,2 

DCMAT ( I ) -DCMAT ( I )+COV( I , J)*H( J) 

110  CONTINUE 
DMAT-VAR 
DO  120  1-1,6 

DMAT-DMAT+DCMAT ( I ) *H( I ) 

12 H  CONTINUE 

C  COMPUTE  GAIN  MATRIX 
DO  140  1-1,5, 2 
GAIN( I )«0. 0 
GAIN( 1+1 )-0. 0 
DO  130  J=1 ,5,2 

GAIN (I ) -GAIN (I )+COV(I,J)*H' J) 

130  CONTINUE 

GAIN( I )«GAIN( I )/DMAT 
140  CONTINUE 
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C  UPDATE  COVARIANCE  MATRIX  AND  LIMIT  THE  GAIN  TO 
C  PREVENT  NEGATIVE  DIAGONAL  COVARIANCES 
DO  150  1-1,5, 2 
DCMAT ( I ) -0 . 0 
DO  150  J-1,5,2 

DCMAT ( I ) -DCMAT ( I ) +H ( J ) *COV( J , I ) 

150  CONTINUE 

DO  160  1-1,5, 2 

POLD( I , I )«COV( I , I )-GAIN{ I ) *DCMAT( I ) 

IF  ( POLD( 1,1)  .LT.  0.0)  THEN 
IF  (COUNT  .LT.  10)  THEN 
DO  155  J-1,5,2 

GAIN ( J ) -GAIN ( J )/2 . 0 
155  CONTINUE 

COUNT-COUNT+1 
KFLAG-KFLAG+1 
GOTO  150 
ELSE 

KFLAG-KFLAG+990 
RETURN 
ENDIF 
END  IF 
JUP-I-2 

DO  160  J-l , JUP,2 

POLD(  I ,  J  )-COV(  I ,  J  )-GAlN  ( I )  *fDCMAT(  J  ) 

160  CONTINUE 

C  UPDATE  ESTIMATE 

XHAT ( 1 ) -XHAT ( 1 ) +GAIN  { 1 ) *RES 
XHAT { 3 ) -XHAT ( 3 ) +GAIN ( 3 ) *RES 
XHAT ( 5 ) -XHAT { 5 ) +GAIN ( 5 ) *RES 

C  UPDATE  COV  MATRIX 
DO  175  1-1,5, 2 

COV(I,I )-POLD(I,I) 

JUP-I-2 

DO  175  J-l , JUP, 2 
COV{ I ,J)-POLD( I, J) 

COV ( J , I ) -POLD ( I , J ) 

175  CONTINUE 


END 


oft 


c 
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C  THIS  IS  A  COLLECTION  OF  SUBROUTINES  NEEDED  FOR 
C  SPLINE  COMPUTATION  AND  NUMERICAL  SEARCH  OF 
C  OPTIMAL  SOLUTION  FOR  THE  HYPERVELOCITY  ORBITAL 
C  INTERCEPT  PROGRAM 


C  SUBROUTINE  DICTIONARY 


C 

A 

PRESENT  ACCELERATION 

OF 

EVADER 

c 

ACOEF 

A 

SPLINE 

COEFFICIENT 

c 

AD 

DUMMY  ACCELERATION 

c 

ASIGX 

A 

SPLINE 

COEFFICIENT 

OF 

SIGMAX 

c 

AS  I  GY 

A 

SPLINE 

COEFFICIENT 

OF 

SIGMAY 

c 

ASIGZ 

A 

SPLINE 

COEFFICIENT 

OF 

SIGMAZ 

c 

AX 

A 

SPLINE 

COEEFICIENT 

OF 

X 

c 

AY 

A 

SPLINE 

COEEFICIENT 

OF 

Y 

c 

AZ 

A 

SPLINE 

COEEFICIENT 

OF 

Z 

c 

BCOEF 

B 

SPLINE 

COEFFICIENT 

c 

BSIGX 

B 

SPLINE 

COEFFICIENT 

OF 

SIGMAX 

c 

BSIGY 

B 

SPLINE 

COEFFICIENT 

OF 

SIGMAY 

C 

BSIGZ 

B 

SPLINE 

COEFFICIENT 

OF 

SIGMAZ 

c 

BX 

B 

SPLINE 

COEEFICIENT 

OF 

X 

C 

BY 

B 

SPLINE 

COEEFICIENT 

OF 

Y 

c 

BZ 

B 

SPLINE 

COEEFICIENT 

OF 

Z 

c 

CCOEF 

C 

SPLINE 

COEFFICIENT 

C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


CFLAG 

COSTl 

COST2 

CSIGX 

CSIGY 

CSIGZ 

CVDD 

CVDR 

CVDY 

CVDZ 

CVD2 

CVDIFY 

CVDIFZ 

CVTOT 

CX 

CY 

CZ 

DA 

DCOEF 

DDY 

DDZ 

DELTAY 

DELTAZ 

DEN 

DSIGX 

DSIGY 

DSIGZ 


CONVERGENCE  FLAG 
DUAL  CONTROL  COST 
DUAL  CONTROL  COST 
C  SPLINE  COEFFICIENT  OF  SIGMAX 
C  SPLINE  COEFFICIENT  OF  SIGMAY 
C  SPLINE  COEFFICIENT  OF  SIGMAZ 
COVARIANCE  MATRIX  FOR  EKF 
COVARIANCE  MATRIX  FOR  EXF 
COVARIANCE  MATRIX  FOR  EKF 
COVARIANCE  MATRIX  FOR  EKF 
COVARIANCE  MATRIX  FOR  EKF 
SELECTED  COVARIANCE  DIFFERENCE 
SELECTED  COVARIANCE  DIFFERENCE 
SELECTED  COVARIANCE  TOTAL 
C  SPLINE  COEEFICIENT  OF  X 
C  SPLINE  COEEFICIENT  OF  Y 
C  SPLINE  COEEFICIENT  OF  Z 
DISTANCE  DUE  TO  BOOSTER  ACCELERATION 
D  SPLINE  COEFFICIENT 
CHANGE  IN  Y  VELOCITY 
VELOCITY 
VELOCITY 
VELOCITY 

DENOMINATOR  (FOR  JACOBIAN  DETERMINANT) 
D  SPLINE  COEFFICIENT  OF  SIGMAX 
D  SPLINE  COEFFICIENT  OF  SIGMAY 
D  SPLINE  COEFFICIENT  OF  SIGMAZ 


CHANGE  IN  Z 
CHANGE  IN  Y 
CHANGE  IN  Z 
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C 

C 

C 

C 

C 

C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


DTGO 

DUMMY 

DVY 

DVYA 

DVZ 

DVZA 

DX 

DY 

DZ 

FSIGX 

FSIGY 

FSIGZ 

Fl 

F2 

F3 

GSIGX 

GSIGY 

GSIGZ 

H 

I 

J 

Jll 

J12 

J13 

J22 

LAMBDA 

MAXDV 

MISS 

MOOT 

OLDTGO 

RADI CL 

RANGE 

SIG 

SIGMAM 

SIGX 

SIGY 

SIGZ 

SIGXDT 

SIGYDT 

SIGZDT 

TD 

TGO 

TG03 

TGO  6 

TLIM 

TOL 

VA 

VE 

vx 

VXE 

VY 

VYE 

VZ 


CHANGE  IN  TIME-TO-GO 
DUMMY  VARIABLE 

Y  VELOCITY  CHANGE  FOR  DEVIATION 

Y  VELOCITY  CHANGE  FOR  DEVIATION 
Z  VELOCITY  CHANGE  FOR  DEVIATION 
Z  VELOCITY  CHANGE  FOR  DEVIATION 
D  SPLINE  COEEFICIENT  OF  X 

D  SPLINE  COEEFICIENT  OF  Y 

D  SPLINE  COEEFICIENT  OF  Z 

SIGMA  X  SPLINE  COEFF  FOR  Y  VEL  CHANGE 

SIGMA  Y  SPLINE  COEFF  FOR  Y  VEL  CHANGE 

SIGMA  Z  SPLINE  COEFF  FOR  Y  VEL  CHANGE 

NONLINEAR  SYSTEM  EQUATION  VALUE 

NONLINEAR  SYSTEM  EQUATION  VALUE 

NONLINEAR  SYSTEM  EQUATION  VALUE 

SIGMA  X  SPLINE  COEFF  FOR  Z  VEL  CHANGE 

SIGMA  Y  SPLINE  COEFF  FOR  Z  VEL  CHANGE 

SIGMA  Z  SPLINE  COEFF  FOR  Z  VEL  CHANGE 

ITERATION  TIME  STEP  SIZE 

COUNTER 

COUNTER 

JACOBIAN  MATRIX  1,1  ELEMENT 
JACOBIAN  MATRIX  1,2  ELEMENT 
JACOBIAN  MATRIX  1,3  ELEMENT 
JACOBIAN  MATRIX  2,2  ELEMENT 
LAGRANGE  MULTIPLIER 

MAXIMUM  PERMISSIBLE  VELOCITY  CHANGE 
MISS  DISTANCE 

UNITIZED  MASS  FLOW  RATE  OF  EVADER 
PREVIOUS  TIME-TO-GO 
QUANTITY  INSIDE  RADICAL  SIGN 
RELATIVE  RANGE 
DEVIATION  AT  FINAL  TIME 
MEASUREMENT  DEVIATIONS 
X  DEVIATION  AT  FINAL  TIME 

Y  DEVIATION  AT  FINAL  TIME 
Z  DEVIATION  AT  FINAL  TIME 
TIME  DERIVATIVE  OF  SIGX 
TIME  DERIVATIVE  OF  SIGY 
TIME  DERIVATIVE  OF  SIGZ 
DUMMY  TIME 

TIME-TO-GO 
3 . 0*TGO 
6 . 0*TGO 

TIME  STEP  LIMIT 
MISS  TOLERANCE 

VELOCITY  DUE  TO  BOOSTER  ACCELERATION 

EVADER  VELOCITY  MAGNITUDE 

EVADER  UNITIZED  X  VELOCITY 

EVADER  X  VELOCITY 

EVADER  UNITIZED  Y  VELOCITY 

EVADER  Y  VELOCITY 

EVADER  UNITIZED  Z  VELOCITY 
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C 

VZE 

EVADER  Z  VELOCITY 

C 

XDUALD 

EKF  STATE  ESTIMATE 

C 

XDUALR 

EKF  STATE  ESTIMATE 

C 

XDUALY 

EKF  STATE  ESTIMATE 

C 

XDUALZ 

EKF  STATE  ESTIMATE 

C 

XDUAL2 

EKF  STATE  ESTIMATE 

c 

XE 

EVADER  RELATIVE  STATE  VECTOR 

c 

XEDD 

EVADER  RELATIVE  STATE  VECTOR 

c 

XEDR 

EVADER  RELATIVE  STATE  VECTOR 

c 

XEDY 

EVADER  RELATIVE  STATE  VECTOR 

c 

XEDZ 

EVADER  RELATIVE  STATE  VECTOR 

c 

XED2 

EVADER  RELATIVE  STATE  VECTOR 

c 

XF 

FINAL  RELATIVE  X  POSITION 

c 

XFDOT 

FINAL  RELATIVE  X  VELOCITY 

c 

XP 

PURSUER  RELATIVE  STATE  VECTOR 

c 

XPDD 

PURSUER  RELATIVE  STATE  VECTOR 

c 

XPDR 

PURSUER  RELATIVE  STATE  VECTOR 

c 

XPDY 

PURSUER  RELATIVE  STATE  VECTOR 

c 

XPDZ 

PURSUER  RELATIVE  STATE  VECTOR 

c 

XPD2 

PURSUER  RELATIVE  STATE  VECTOR 

c 

XR 

RELATIVE  STATE  VECTOR 

c 

XO 

INITIAL  RELATIVE  X  POSITION 

c 

XI 

RELATIVE  X  POSITION  AT  ONE  THIRD  POINT 

c 

X2 

RELATIVE  X  POSITION  AT  TWO  THIRDS  POINT 

c 

X3 

RELATIVE  X  POSITION  AT  FINAL  POINT 

c 

XODOT 

INITIAL  RELATIVE  X  VELOCITY 

c 

YF 

FINAL  RELATIVE  Y  POSITION 

c 

YFDOT 

FINAL  RELATIVE  Y  VELOCITY 

c 

ZF 

FINAL  RELATIVE  Z  POSITION 

c 

ZFDOT 

FINAL  RELATIVE  Z  VELOCITY 

SUBROUTINE  SPLINE4 ( XO , XI , X2 , X3 , TGO , 

+  ACOEF , BCOEF , CCOEF , DCOEF ) 

C  THIS  SUBROUTINE  CREATES  THE  SPLINE  COEFFICIENTS 
C  FOR  THE  EQUATION: 

C  ACOEF*T**3  +  BCOEF*T**2  +  CCOEF*T  +  DCOEF 

C  GIVEN  FOUR  EQUI SPACED  POINTS  AND  TIME-TO-GO. 

C  PRESENT  TIME  IS  EQUAL  TO  ZERO. 

REAL *8  XO , XI , X2 , X3 , TGO , ACOEF , BCOEF , CCOEF , DCOEF 

ACOEF- ( -4 . 5*X0  +13.5*X1  -13.5*X2  +4.5*X3)/ 

+  ( TGO*TGO*TGO ) 

BCOEF- ( +9 . 0*X0  -22 . 5*X1  +18 . 0*X2  -4.5*X3)/ 

+  (TGO*TGO) 

CCOEF-( -5 . 5*X0  +9 .  0*X1  -4 . 5*X2  +1.0*X3)/TGO 

DCOEF-XO 

END 


D-4  5 


SUBROUTINE  SPLINE ( XO ,  XODOT ,  XF , XFDOT , TOO , 

+  ACOEF , BCOEF , CCOEF , DCOEF ) 

C  THIS  SUBROUTINE  CREATES  THE  SPLINE  COEFFICIENTS 
C  FOR  THE  EQUATION: 

C  ACOEF*T**3  +  BCOEF*T**2  +  CCOEF*T  +  DCOEF 

C  GIVEN  INITIAL  POSITION  AND  VELOCITY  AND 
C  FINAL  POSITION/  VELOCITY,  AND  TIME-TO-GO. 

C  PRESENT  TIME  IS  EQUAL  TO  ZERO. 

REAL*8  XO, XODOT, XF, XFDOT, TGO 
REAL *8  ACOEF, BCOEF, CCOEF, DCOEF 

DCOEF-XO 

CCOEF-XODOT 

ACOEF- (  2 . 0  *  ( XO-XF ) /TGO+XODOT+XFDOT ) /TGO/TGO 
BCOEF- ( 3 . 0* { XF-XO )/TGO-2 . 0*XODOT-XFDOT )/TGO 

END 


SUBROUTINE  SEARCHA ( AX , BX , CX , DX , AY , B Y , CY , DY , 

+  AZ , BZ , CZ , DZ , K , TGO , DELTAY , DELTAZ , TOL , CFLAG ) 

C  THIS  SUBROUTINE  NUMERICALLY  MINIMIZES  THE 
C  COST  FUNCTION: 

C  L  -  K* ( XF**2+XY**2+XZ**2 )/2  + 

C  (DELTAX**2  +  DELTAY* *2 )/2 

C  BY  VARYING  THE  TIME-TO-GO  (TGO)  AND  THE  VELOCITY 
C  CHANGES  (DELTAY  AND  DELTAZ)  TO  BRING  THE  MISS 
C  DISTANCE  WITHIN  SOME  TOLERANCE. 

C  XF, YF  AND  ZF  ARE  COMPUTED  FROM  THE  SPLINE 
C  COEFFICIENTS  AX,BX,...,DZ  AT  THE  FINAL  TIME. 

C  THIS  IS  ACCOMPLISHED  BY  EMPLOYING  A  NEWTON-RAPHSON 
C  SEARCH  SCHEME  FOR  NON-LINEAR  SYSTEMS. 

C  (PP.  176-179  OF  MARON,  'NUMERICAL  ANALYSIS, 

C  A  PRACTICAL  APPROACH') 

REAL *8  AX , BX , CX , DX , AY , BY , CY , DY , AZ , BZ , CZ , DZ , DDY 
REAL *8  K , TGO , DELTAY , DELTAZ , XF , YF , ZF , Fl , F2 , F3 
REAL*8  XFDOT , YFDOT , Z FOOT ,J11,J12,J13,J22, TGO 3 
REAL* 8  MISS, TOL , TG06 , OLDTGO , SCALE , DEN , DTGO , DDZ 
INTEGER  I, CFLAG 

C  INITIALIZE  VELOCITY  CHANGES  AND  OLD  FINAL  TIME 
DELTAY-0 . 0 
DELTAZ-0 . 0 
OLDTGO-TGO 

C  BEGIN  SEARCH  LOOP 
DO  10  1-1,25 
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C  COMPUTE  FINAL  RELATIVE  POSITIONS 

XF-( (AX*TGO+BX)*TGO+CX)*TGO+DX 

YF- ( { AY*TGO+BY ) *TGO+CY-DELTAY ) *TGO+DY 

2F- ( ( AZ*TGO+BZ ) *TGO+CZ-DELTAZ ) *TGO+DZ 

MISS-SQRT(XF*XF+YF*YF+ZF*ZF) 

C  TEST  FOR  NEARNESS  (THIS  IS  RANGE  RELATIVE) 

IF  (MISS  .LE.  TOL)  RETURN 

C  COMPUTE  FINAL  RELATIVE  VELOCITIES 
TGO3-TGO+TG0+TGO 
XFDOT- ( AX*TG03+BX+BX ) *TGO+CX 
YFDOT- ( AY*TG03+BY+BY ) *TGO+CY-DELTAY 
ZFDOT-( AZ*TG03+BZ+BZ ) *TGO+CZ-DELTAZ 

C  COMPUTE  NONLINEAR  SYSTEM  EQUATIONS 

Fl«K*(XF*XFDOT+YF*YFDOT+ZF*ZFDOT) 

F2-DELTAY-K*YF*TG0 

F3«DELTAZ-K*ZF*TG0 

C  COMPUTE  NECESSARY  ELEMENTS  OF  JACOBIAN  MATRIX 
TG06-TG03+TG03 

Jll-K* ( XF* { AX*TG06+BX+BX ) +YF* ( AY*TG06+BY+BY ) + 
+  ZF* ( AZ*TG06+BZ+BZ )+XFDOT*XFDOT+ 

+  YFDOT* YFDOT+ZFDOT*ZFDOT) 

J12— K* ( YF+YFDOT*TGO ) 

J13— K*(ZF+ZFDOT*TGO) 

J22«1.0+K*TGO*TGO 

C  COMPUTE  CHANGES  IN  CONTROL  PARAMETERS 
DEN«( J11*J22-J13*J13-J12*J12) 

DTGO— (Fl*J22-Jl2*F2-Jl3*F3)/DEN 
DDY«-(- J12*F1+ ( J11-J13* J13/J22 )  *F2+ 

+  F3*J12* J13/J22 )/DEN 

DDZ»-(-Jl3*Fl+F2*J12* J13/J22+ 

+  ( Jll-Jl2*Jl2/J22)*F3)/DEN 

C  UPDATE  CONTROL  PARAMETERS 
TGO-TGO+DTGO 
DELTAY-DELTAY+DDY 
DELTAZ-DELTAZ+DD2 

C  TEST  FOR  CONVERGENCE  OF  FINAL  TIME 

IF  (ABS(DTGO)  .LT.  (. 000001 *TGO ) )  RETURN 

10  CONTINUE 

C  INCREMENT  CONVERGENCE  FLAG 
CFLAG-CFLAG+1 

C  ZERO  OUT  VELOCITY  CHANGES  AND  RESET  TIME 
DELTAY-0.0 
DELTAZ-0.0 
TGO-OLDTGO 
END 
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SUBROUTINE  SEARCHB ( AX , BX , CX , DX , AY , BY , CY , DY , 

+  AZ , BZ , CZ , DZ , TOO , DELTAY , DELTAZ , TOL , CFLAG ) 

C  THIS  SUBROUTINE  FINDS  THE  TIME-TO-GO  AND  VELOCITY 
C  CHANGES  WITH  MISS  DISTANCE  WITHIN  SOME  EXTERNAL 
C  TOLERANCE. 

C  THIS  IS  DONE  BY  VARYING  THE  TIME-TO-GO  ( TGO)  TO 
C  VARY  THE  X  MISS  DISTANCE. 

C  THIS  IS  ACCOMPLISHED  BY  EMPLOYING  A  NEWT ON-RAPHSON 
C  SEARCH  SCHEME  (PP.  48-53  OF  MARON,  'NUMERICAL 
C  ANALYSIS,  A  PRACTICAL  APPROACH') 

C  XF , AND  XFDOT  ARE  COMPUTED  FROM  THE  SPLINE 
C  COEFFICIENTS  AX,BX,CX,DX  AT  THE  FINAL  TIME. 

C  ONCE  THE  FINAL  TIME  IS  KNOWN  THE  VELOCITY  CHANGES 
C  (DELTAY  AND  DELTAZ)  ARE  COMPUTED. 

REAL  *  8  AX , BX , CX , DX , AY , B Y , CY , DY , AZ , BZ , CZ , DZ , DTGO 
REAL *8  TGO , DELTAY , DELTAZ , XFDOT , SCALE , XF , YF , ZF 
REAL *8  MISS, TOL, DDY , DDZ , OLDTGO 
INTEGER  I, CFLAG 

C  INITIALIZE  VARIABLES 
DELTAY- 0 . 0 
DELTA Z-0.0 
OLDTGO- TGO 

C  BEGIN  SEARCH  LOOP 
DO  10  1-1,10 

C  COMPUTE  FINAL  POSITIONS  AND  MISS  DISTANCE 
XF- ( ( AX*TGO+BX ) *TGO+CX ) *TGO+DX 
YF- ( ( AY*TGO+BY ) *TGO+CY-DELTAY ) *TGO+DY 
ZF- ( ( AZ*TGO+BZ ) *TGO+CZ-DELTAZ ) *TGO+DZ 
MISS-SQRT( XF*XF+YF*YF+ZF*ZF ) 

C  TEST  FOR  NEARNESS 

IF  (MISS  .LE.  TOL)  RETURN 

C  COMPUTE  CHANGE  IN  TIME-TO-GO 

XFDOT- ( AX* ( TGO+TGO+ TGO ) +BX+BX ) *TGO+CX 
DTGO— XF/XFDOT 
TGO- TGO  DTGO 

C  COMPUTE  CHANGES  IN  VELOCITIES 

YF- ( ( AY*TGO+BY ) *TGO+CY-DELTAY ) *TGO+DY 
ZF- ( ( AZ*TGO+BZ ) *TGO+CZ-DELTAZ ) *TGO+DZ 
DDY- ( AY*TGO+BY ) *TGO+CY-DELTAY+DY/TGO 
DELTAY-DELTAY+DDY 

DDZ- ( AZ  *TGO+BZ ) *TGO+CZ-DELTAZ+DZ/TGO 
DELTAZ-DELTAZ+DDZ 

C  TEST  FOR  CONVERGENCE  OF  FINAL  TIME 

IF  (ABS(DTGO)  .LT.  ( . 000001*TGQ) )  RETURN 
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10  CONTINUE 

C  INCREMENT  CONVERGENCE  FLAG 
CFLAG-CFLAG+1 

C  ZERO  OUT  VELOCITY  CHANGES  AND  RESET  TIME 
DELTAY- 0.0 
DELTAZ-0 . 0 
TGO-OLDTGO 

END 


SUBROUTINE  SEARCHC ( XR , VXE , VYE , VZE , A , MOOT , 

+  TGO , DELTAY , DELTAZ , TOL , CFLAG ) 

C  THIS  SUBROUTINE  DETERMINES  TIME-TO-GO  AND  VELOCITY 

C  CHANGES  FOR  PLAN  C  WITH  MISS  DISTANCE  WITHIN  SOME 

C  TOLERANCE. 

C  THIS  IS  ACCOMPLISHED  BY  EMPLOYING  A  NEWTON-RAPHSON 

C  SEARCH  SCHEME  (PP.  48-53  OF  MARON,  'NUMERICAL 

C  ANALYSIS,  A  PRACTICAL  APPROACH') 

C  ONCE  THE  TIME-TO-GO  IS  KNOWN  THE  VELOCITY  CHANGES 

C  (DELTAY  AND  DELTAZ)  ARE  COMPUTED. 

REAL *8  DTGQ,XR{ 6 ) ,VXE,VYE,VZE,A,MDOT,DA,VE 
REAL* 8  TF , DELTAY , DELTAZ , XF , YF , ZF , XFDOT , SCALE 
REAL*  8  OLDTGO , VX , VY , VZ , MI SS , TGO , TOL , MT 
INTEGER  I, J, CFLAG 

C  COMPUTE  EVADER  UNITIZED  VELOCITY  VECTOR  COMPONENTS 
VE-SQRT{ VXE*VXE+VYE*VYE+VZE*VZE ) 

VX-VXE/VE 

VY-VYE/VE 

VZ-VZE/VE 

C  INITIALIZE  VARIABLES 
DELTAY-0.0 
DELTAZ-0.0 
OLDTGO* TGO 

C  COMPUTE  DISTANCE  DUE  TO  BOOSTER  ACCELERATION 
DA-0.0 
MT-NDOT*TGO 
DO  15  J-2,50 

DA-DA+MT**J/( J*{ J-l) ) 

15  CONTINUE 

DA-DA* A/ ( MDOT*MDOT ) 

C  BEGIN  SEARCH  LOOP 
DO  30  1-1,10 


o  r>  o  o  o 
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C  COMPUTE  FINAL  RELATIVE  POSITION  AND  MISS  DISTANCE 
XF-XR(1 )+XR(2 )*TGO+DA*VX 
YF-XR ( 3 ) + ( XR( 4 ) -DELTAY ) *TGO+DA*VY 
ZF«XR( 5 ) + ( XR( 6 ) -DELTAZ ) *TGO+DA*V2 
MISS-SQRT(XF*XF+YF*YF+ZF*ZF) 

C  TEST  FOR  NEARNESS 

IF  (MISS  .LE.  TOL)  RETURN 

C  COMPUTE  CHANGE  IN  TIME-TO-GO 
XFDOT-XR ( 2 ) +DA* VX/TGO 
DTGO— XF/XFDOT 
TGO-TGO+DTGO 

C  COMPUTE  CHANGES  IN  VELOCITIES 
DA-0 . 0 
MT«MDOT*TGO 
DO  ? 5  J-2,50 

DA-DA+MT**J/( J*( J-l) ) 

25  CONTINUE 

DA-DA* A/ ( MDOT*MDOT ) 

DELTAY- ( DA* VY+XR ( 3 ) ) /TGO+XR ( 4 ) 

DELTAZ- ( DA*VZ+XR ( 5 ) ) /TGO+XR ( 6 ) 

C  TEST  FOR  CONVERGENCE  OF  TIME-TO-GO 

IF  (ABS(DTGO)  .LT.  ( . 000001*TGO) )  RETURN 

30  CONTINUE 

C  INCREMENT  CONVERGENCE  FLAG 
CFLAG-CFLAG+1 

C  ZERO  OUT  VELOCITY  CHANGES  AND  RESET  TIME 
DELTAY-0 . 0 
DELTAZ-0 . 0 
TGO-OLDTGO 

END 


SUBROUTINE  SEARCHCC ( AX , BX , CX , DX , AY , BY , CY , DY , 

+  AZ , BZ , CZ , DZ , AS IGX , BSIGX , CSIGX , DS1GX , ASIGY , 
+  BSIGY , CSIGY , DS IGY , AS IGZ , BSIGZ , CSIGZ , DSIGZ  f 
+  K , TGO , DELTAY , DELTAZ , TOL , CFLAG ) 

THIS  SUBROUTINE  NUMERICALLY  MINIMIZES  THE 
COST  FUNCTION: 

L  -  ( DELTAX**2  +  DELTAY**2)/2 
SUBJECT  TO  THE  CONSTRAINT  : 

XF*XF+YF*YF+ZF*ZF  <-  K* ( SIGMAF*SIGMAF) 
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C  BY  VARYING  THE  TIME-TO-GO  ( TGO )  AND  THE  VELOCITY 
C  CHANGES  ( DELTAY  AND  DELTAZ ) . 

C  XF,YF  AND  ZF  ARE  COMPUTED  FROM  THE  SPLINE 
C  COEFFICIENTS  AX,BX,...,DZ  AT  TEE  FINAL  TIME, 

C  AS  ARE  THE  FINAL  SIGMAS. 

C  THIS  IS  ACCOMPLISHED  BY  EMPLOYING  A  NEWTON-RAPHSON 
C  SEARCH  SCHEME  FOR  NON-LINEAR  SYSTEMS. 

C  (PP.  176-179  OF  MARQN,  ' NUMERICAL  ANALYSIS, 

C  A  PRACTICAL  APPROACH') 

REAL *8  AX ,  BX ,  CX ,  DX ,  AY .  BY ,  CY  ,  DY ,  AZ  ,  BZ  ,  CZ  ,  D‘Z 
REAL*8  DTGO , TLIM, SCALE , DEN, OLDTGO , KSIG 
REAL*8  K , TGO , DELTAY , DELTAZ , XF , YF , ZF , FI , F2 , F3 
REAL *8  XFDOT, YFDOT, ZFDOT, Jll ,JI2,J21,J22, TG03 
REAL *8  ASIGX , BSIGX, CSIGX, DSIGX, ASIGY,BSIGY 
REAL *8  CSIGY,DSIGY,DLAM, PERT, MISS, FIT, F2T,F1L,F2L 
REAL*  8  TOL , AS I GZ , BS I GZ , CS I GZ , DS I GZ , LAMBDA 
REAL* 6  SIGX, SIGY, SIGZ , SIGXDT, SIGYDT, SIGZDT 
REAL *8  DYDL , DZDL , DYDTDL , DZDTDL , TG06 , YS , ZS 
REAL  *  8  SI GXDD ,SIGYDD,SIGZDD, XFDD , YFDL , Z  FDD 
INTEGER  I , J , CFLAG 

C  INITIALIZE  PARAMETERS 
FERT-0. 0000001 
OLDTGG-TGO 
TLIM-0.01*TGO 
DELTAY-0 . 0 
DELTAZ-0.0 

C  DETERMINE  IF  CONSTRAINT  CAN  BE  SATISFIED 
C  WITHOUT  THRUSTING 

DO  5  I»l,10 

C  COMPUTE  FINAL  RELATIVE  POSITIONS  AND  DEVIATIONS 

XF-( ( AX*TGO+BX ) *TGO+CX ) *TGO+DX 
YF-(  ( AY*TGO+BY) *TGO+CY)  *TGO+DY 
ZF-  {  ( AZ*TGO+BZ  )  *TGO+CZ  )  *TGO+D7. 

SIGX- (  ( ASIGX*TGO+BSIGX)  *TGO-f  CSIGX)  *TGO+DSIGX 
SIGY-( (ASIGY*TGO+BSIGY)*TGO+CSIGY)*TGOvDSIGY 
SIGZ-(  ( ASIGZ*TGO+ESIGZ  )  *TGO*CSIGZ  )  *TGO+DSIGZ 

C  COMPUTE  FINAL  RELATIVE  VELOCITIES 

C  AND  DEVIATION  DERIVATIVES 

TGO3-TGO+TGO+TG0 
XFDOT-(AX*TG03+BX+BX)*TGO+CX 
YFDOT- { AY*  TG03+BY+BY ) *TGO+CY 
ZFDOT- ( AZ *TG03+BZ+BZ )*TGO+CZ 
SIGXDT-(ASIGX*TG03+BSIGX+BSIGX)*TGO+CSIGX 
SIGYDT- (ASIGY*TG03+BSIGY+BSIGY)*TGO+CSIGY 
SIGZDT-( ASIGZ*TG03+BSIGZ+BSIGZ ) *TGO+CSIGZ 


o  c 
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C  COMPUTE  NONLINEAR  SYSTEM  EQUATIONS 

Fl-(XF*XF+YF*YF+ZF*ZF-K*(SIGX*SIGX+ 

+  SIGY*SIGY+SIGZ*SIGZ ) )/2.0 

C  DETERMINE  IF  CONSTRAINT  IS  SATISFIED 

IF  (FI  .LT.  TOL)  RETURN 

F2-XF*XFDOT+YF*YFDOT+ZF*ZFDOT-K* ( SIGX*SIGXDT+ 
+  SIGY*SIGYDT+SIGZ*SIGZDT) 

F3“XFDOT*XFDOT+YFDOT*YFDOT+ZFDOT*ZFDOT-K* 

+  ( SIGXDT*SIGXDT+SIGYDT*SIGYDT+SIGZDT*SIGZDT) 

C  COMPUTE  NEW  TGO 
DTGO— F2/F3 
TGO-TGO+DTGO 

C  TEST  FOR  CONVERGENCE  OF  FINAL  TIME 

IF  (ABS(DTGO)  .LT.  { TOL*TLIM) )  GOTO  6 

5  CONTINUE 

6  CONTINUE 

C  INITIALIZE  LAMBDA 

MISS-SQRT(XF*XF+YF*YF+ZF*ZF) 

KSIG-SQRT(K*(SIGX*SIGX+SIGY*SIGY+SIGZ*SIGZ) ) 
LAMBDA- (1 . 0 - K  S I G/M I S  S ) /TGO/TGO 

C  BEGIN  SEARCH  LOOP 
DO  15  1-1,20 

C  COMPUTE  VELOCITY  CHANGES 

YS-( (AY*TGO+BY)*TGO+CY)*TGO+DY 
ZS«( ( AZ*TGO+BZ ) *TGO+CZ ) *TGO+DZ 
DEN-1 . 0+LAMBDA*TGO*TGO 
DELTAY-YS*TGO* LAMBDA/DEN 
DELTAZ-ZS*TGO* LAMBDA/DEN 

C  COMPUTE  FINAL  RELATIVE  POSITIONS  AND  DEVIATIONS 
XF- ( ( AX*TGO+BX ) *TGO+CX ) *TGO+DX 
YF«YS-DELTAY*TGO 
ZF-ZS-DELTAZ*TGO 

SIGX-( ( ASIGX*TGO+BSIGX ) *TGO+CSIGX) *TGO+DSIGX 
SIGY«( ( ASIGY*TGO+BSIGY ) *TGO+CSIGY ) *TGO+DSIGY 
SIGZ-( ( ASIGZ*TGO+BSIGZ ) *TGO+CSIGZ ) *TGO+DSIGZ 

COMPUTE  FINAL  RELATIVE  VELOCITIES 
AND  DEVIATION  DERIVATIVES 
TG03-TG0+TG0+TG0 
XFD0T«(AX*TG03+BX+BX)*TG0+CX 
YFDOT- ( AY*TG03+BY+BY ) *TGO+CY-DELTAY 
ZFDOT-{ A2*TG03+BZ+BZ ) *TGO+CZ-DELTA2 
S I GXDT- ( AS I GX  *  TGO  3 +BS I GX+ BS I GX ) *  TGO+  CS I GX 
SIGYDT-(ASIGY*TG03+BSIGY+BSIGY)*TG0+CSIGY 
SIGZDT-( ASIGZ*TG03+BSIGZ+BSIGZ ) *TGO+CSIGZ 
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C  COMPUTE  NONLINEAR  SYSTEM  EQUATIONS 

Fl«(XF*XF+YF*YF+ZF*ZF-X*(SIGX*SIGX+ 

+  SIGY*SIGY+SIGZ*SIGZ ) )/2 . 0 

F2«XF*XFDOT+YF*YFDOT+ZF*ZFDOT-K* 

+  ( SIGX*SIGXDT+SIGY*SIGYDT+SIGZ*SIGZDT) 

C  TEST  FOR  CONVERGENCE  OF  CONSTRAINTS 
IF  ((ABS(Fl)  .LT.  TOL)  .AND. 

+  ( ABS ( F2 )  .LT.  TOL))  RETURN 

C  COMPUTE  NECESSARY  PARTI ALS 
DYDTDL*-YF*TGO/DEN 
DZDTDL— ZF*TGO/DEN 
DYDL-DYDTDL*TGO 
DZDL-DZDTDL*TGO 
TG06-TG03+TG03 
XFDD-AX*TG06+BX+BX 

YFDD-AY*TG06+BY+BY-LAMBDA* ( YFDOT*TGO+YF ) 

ZFDD*AZ*TG06+BZ+BZ-LAMBDA*(ZFDOT*TGO+ZF) 

SIGXDD-ASIGX*TG06+BSIGX+BSIGX 

SIGYDD-ASIGY*TG06+BSIGY+BSIGY 

SIGZDD»ASIGZ*TG06+BSIGZ+BSIGZ 

C  COMPUTE  NECESSARY  ELEMENTS  OF  JACOBIAN  MATRIX 
J11»F2 

J12-YF*DYDL+ZF*DZDL 

J21«XF*XFDD+XFDOT*XFDOT-fYF*YFDD+YFDOT*YFDOT+ 

ZF*ZFDD+ZFDOT*ZFDOT 

J21-J21-K*(SIGX*SIGXDD+SIGXDT*SIGXDT+ 

+  SIGY*SIGYDD+SIGYDT*SIGYDT+SIGZ*SIGZDD+ 

+  SIGZDT*SIGZDT) 

J22-DYDL*YFDOT+YF*DYDTDL+DZDL*ZFDOT+ZF*DZDTDL 

C  COMPUTE  CHANGES  IN  CONTROL  PARAMETERS 
DEN-( Jll*J22-Jl2*J21 ) 

DTGO*{  ~F1*  J22-‘-F2*  J12  )/DEN 
DLAN-{F1*J21-F2*J11 )/DEN 
SCALE»ABS ( DTGO/TL I M ) 

IF  (SCALE  .GT.  1.0)  THEN 
DTGO-DTGO/SCALF 
DLAM-DLAM/SCALE 
END  IF 

C  UPDATE  CONTROL  PARAMETERS 
TGO-TGO+DTGO 
LAMBDA*  LAMBDA -fDLAM 

15  CONTINUE 


C  INCREMENT  CONVERGENCE  FLAG 
CFLAG-CFLAG+1 
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C  ZERO  OUT  VELOCITY  CHANGES  AND  RESET  TIME 
DELTAY- 0.0 
DELTAZ=0 . 0 
TGO-OLDTGO 

END 


SUBROUTINE  SEARCHD ( AX , BX , CX , DX , AY , BY , CY , DY , AZ , 

+  BZ , CZ , DZ , K , TGO , DELTAY , DELTAZ , TOL , CFLAG , XDUALD , 
+  XEDD , XPDD , CVDD , MAXDV , COUNT ,Q,R3, SIGMAM , H ) 

C  THIS  SUBROUTINE  NUMERICALLY  MINIMIZES  THE 
C  COST  FUNCTION: 

C  L  *  E{K*(XF**2+XY**2+XZ**2)/2  + 

C  ( DELTAX**2  +  DELTAY* *2 )/2 } 

C  BY  VARYING  THE  TIME-TO-GO  (TGO)  AND  THE  VELOCITY 
C  CHANGES  (DELTAY  AND  DELTAZ)  TO  BRING  THE  MISS 
C  DISTANCE  WITHIN  SOME  TOLERANCE. 

C  XF , YF  AND  ZF  ARE  COMPUTED  FROM  THE  SPLINE 
C  COEFFICIENTS  AX,BX,...,DZ  AT  THE  FINAL  TIME. 

REAL* 8  AX , BX , CX , DX , AY , B Y , CY , DY , AZ , BZ , CZ , DZ 
REAL *8  K , TGO , DELTAY , DELTAZ , DH , Q ( 8 , 8 ) ,RES(3) 

REAL *8  DVY , DVZ , XDUALD ( 8 ) , CVDD (8,8), XPDD ( 6 ) 

REAL* 8  MISS, TOL, XEDD(6) ,R3(3, 3) ,TD, RANGE, DVYA 
REAL* 8  SIGMAM ( 3 ) , C0ST1 , COST2 , CVTQT, DVZA 
REAL*8  XDUAL2 ( 8 ) , CVD2 (8,8), XED2 ( 6 ) , XPD2 ( 6 ) 

REAL* 8  CVDY (8,8), CVDZ (8,8), XEDY ( 6 ) , XEDZ ( 6 ) 

REAL*8  XPDY ( 6 ) , XPDZ ( 6 ) , XDUALY ( 8 ) , XDUALZ ( 8 ) 

REAL* 8  XF , YF , ZF , XFDOT , YFDOT , Z FDOT , MAXDV , DTGO 
REAL* 8  CVDR (8,8), XEDR ( 6 ) , XPDR ( 6 ) , XDUALR ( 8 ) 

REAL *8  DEN , CVDI FY , CVDI FZ , RADI CL , RTGO 
INTEGER  I, J, CFLAG, COUNT, K FLAG 

C  C  'MPUTE  CERTAINTY  EQUIVALENCE  SOLUTION 

CALL  SEARCHA ( AX , BX , CX , DX , AY , BY , CY , DY , 

+  AZ , BZ , CZ , DZ , K , TGO , DELTAY , DELTAZ , TOL , CFLAG ) 

C  ASSIGN  DUMMY  FILTER  VARIABLES 
DO  100  1-1,8 

XDUAL2 ( I ) -XDUALD (I ) 

XDUALY ( I ) -XDUALD ( I ) 

XDUALZ ( I ) -XDUALD ( I ) 

XDUALR ( I ) -XDUALD ( I ) 

DO  100  3-1,8 

CVD2( I ,3 )-CVDD( I , J) 

CVDY ( I , J ) -CVDD ( I , J ) 

CVDZ ( I , J ) -CVDD ( I , J ) 

CVDR { I , J ) -CVDD ( I , J ) 

100  CONTINUE 
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DO  110  1=1,6 

XED2 ( I ) =XEDD (  I ) 

XEDY ( I ) =XEDD(  1 ) 

XEDZ ( I ) =XEDD  (  I ) 

XEDR( I ) =XEDD( I ) 

XPD2 ( I ) =XPDD( I ) 

XPDY ( I )=XPDD( I ) 

XPDZ ( I ) =XPDD ( I ) 

XPDR( I ) =XPDD( 1 ) 

110  CONTINUE 

C  DETERMINE  BEST  DIRECTION  TO  IMPROVE  THE  ESTIMATE 
DVY=MAXDV 
DVZ=MAXDV 

XPDY ( 4 ) =XPDY ( 4 ) +DVY 
XPDZ ( 6 ) =XPDZ ( 6 } +DVZ 
XDUALY ( 4 ) =XDUALY ( 4 ) -DVY 
XDUALZ { 6 ) “XDUALZ ( 6 ) -DVZ 

DH-TGO/COUNT 
DO  300  1=1, COUNT 

RANGE=SQRT ( XDUALR ( 1 ) * XDUALR ( 1 ) + 

+  XDUALR ( 3 } * XDUALR ( 3 ) +XDUALR( 5 ) * XDUALR ( 5 ) ) 

R3 ( 1 , 1 )-SIGMAM( 1 ) *  RANGE  *  S I GMAM ( 1 ) * RANGE 
CALL  EKF8 ( XDUALR , XEDR , XPDR , TD , DH , CVDR ,Q,R3, 
+  0.0, 0.0, 0.0, KFLAG , RES , 2 ) 

RANGE=SQRT ( XDUALY ( 1 ) *XDUALY ( 1 )  + 

+  XDUALY ( 3 ) *XDUALY ( 3 ) +XDUALY ( 5 ) * XDUALY ( 5 ) ) 

R3 ( 1 , 1 )-SIGMAM( 1 ) * RANGE* SI GMAM ( 1 ) * RANGE 
CALL  EKF8 ( XDUALY , XEDY , XPDY , TD , DH , CVDY , Q , R3 , 
+  0,0, 0.0, 0.0, KFLAG, RES, 2) 

RANGE=SQRT ( XDUALZ ( 1 ) *XDUALZ ( 1 ) + 

+  XDUALZ ( 3 ) *XDUALZ ( 3 ) +XDUALZ ( 5 ) *XDUALZ ( 5 ) ) 

R3 ( 1 , 1 )«SIGMAM( 1 ) *  RANGE  *  S I GMAM ( 1 ) * RANGE 
CALL  EKF8 ( XDUALZ , XEDZ , XPDZ , TD , DH , CVDZ , Q,R3 , 
+  0.0, 0.0, 0.0, KFLAG, RES, 2) 

300  CONTINUE 

CVTOT-CVDR (1,1) +CVDR (3,3) +CVDR (5,5) 

CVDI FY-CVTOT-CVDY (1,1) -CVDY (3,3) -CVDY (5,5) 
CVDI FZ-CVTOT-CVDZ (1,1) -CVDZ (3,3) -CVDZ (5,5) 

D  EN-CVD I F Y  *  C VD I F Y + C VD I F  Z  *  CVD I FZ 
IF  (DEN  ,GT.  1.03-20)  THEN 
DEN=SQRT( DEN ) 

DVY=DVy*CVDI FY/DEN 
DVZ“DVZ*CVDIFZ/DEN 
ELSE 
RETURN 
END  IF 

C  ENSURE  C.E.  SOLUTION  IS  WITHIN  THRUST  LIMITS 
RTGO=TGO-H 

RADICL=RTGO*RTGO-  (  DELTAY+DELTAY )  *RTGO*H/M/\XDV 


IF  ( RADI CL  .LE.  0.0)  THEN 

DELTAY=MAXDV*INT ( RTGO/H )/2 . 0 
COSTl= ( DELTAY+DELTAY ) **2 
ELSE 

COSTl= ( (RTGO-SQRT(RADICL) )  *MAXDV/H ) **2 
ENDIF 

RADICL=RTGO*RTGO- ( DELTAZ+DELTAZ ) *RTGO*H/MAXDV 
IF  ( PADICL  .LE.  0.0)  THEN 

DELTAZ-MAXDV*  INT  (  RTGO/H )  /2 . 0 
COSTl=COSTl+( DELTAZ+DELTAZ ) **2 
ELSE 

COST1-COST1+ ( (RTGO-SQRT(RADICL) ) *MAXDV/H ) **2 
ENDIF 

C  COMPUTE  COST  OF  CERTAINTY  EQUIVALENCE  SOLUTION 
XDUALD { 4 ) -XDUALD ( 4 ) -DELTAY 
XDUALD ( 6 ) -XDUALD ( 6 ) -DELTAZ 
XPDD ( 4 ) -XPDD ( 4  DELTAY 
XPDD ( 6 ) -XPDD ( 6 ) +DELTAZ 
TD-0 . 0 
DH-H 

RANGE-SQRT ( XDUALD ( 1 ) *XDUALD ( 1 ) + 

+  XDUALD ( 3 ) *XDUALD ( 3 ) +XDUALD ( 5 ) *XDUALD (  5 )  ) 

R3 ( 1 , 1 ) =SIGMAM( 1 ) * RANGERS I GMAM( 1 ) * RANGE 
CALL  EKF8 ( XDUALD , XEDD , XPDD , TD , DH , CVDD , Q , R3 , 

+  0.0,0. 0,0.0, KFLAG , RES , 2 ) 

DTGO-TGO-DH 
DH-DTGO/COUNT 
DO  200  1=1 , COUNT 

RANGE-SQRT ( XDUALD ( 1 ) * XDUALD ( 1 ) + 

+  XDUALD ( 3 ) * XDUALD ( 3 ) +XDUALD( 5 ) * XDUALD { 5 )  ) 

R3 ( 1 , 1 )-SIGMAM( 1 ) *  RANGE  *  S I GMAM ( 1 ) * RANGE 
CALL  EKF8 ( XDUALD , XEDD , XPDD , TD , DH , CVDD , Q , R3 , 

+  0.0, 0.0, 0.0, KFLAG , RES , 2 ) 

200  CONTINUE 

CVTOT-CVDD (1,1) +CVDD (3,3) +CVDD (5,5) 
COSTl-COSTl+K* ( CVTOT+XDUALD ( 1 ) * XDUALD ( 1 ) + 

+  XDUALD( 3 ) * XDUALD ( 3 )+XDUALD( 5 ) *XDUALD( 5 ) ) 

C  DETERMINE  COST  OF  IMPROVING  THE  ESTIMATE 
XPD2 ( 4 ) -XPD2 ( 4 ) +DVY 
XPD2 ( 6 ) -XPD2 { 6 ) +DVZ 
XDUAL2 ( 4 ) -XDUAL2 ( 4 )-DVY 
XDUAL2 ( 6 ) “XDUAL2 ( 6 ) -DVZ 

C  MOVE  FORWARD  ONE  STEP 
DH-H 

RANGE-SQRT( XDUAL2 ( 1 ) *XDUAL2 ( 1 ) + 

+  XDUAL2 ( 3 ) *XDUAL2 ( 3 ) +XDUAL2 ( 5 ) *XDUAL2 ( 5 ) ) 

R3 ( 1 , 1 ) -SI GMAM ( 1 ) GRANGE* SI GMAM ( 1 ) * RANGE 
CALL  EKF8 ( XDUAL2 , XED2 , XPD2 , TD , DH , CVD2 ,Q,R3 , 

+  0.0, 0.0, 0.0, KFLAG, RES, 2) 
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C  COMPUTE  NEW  SPLINES 

CY-CY-DVY 
CZ-CZ-DVZ 

XF-( ( AX*TGO+BX ) *TGO+CX ) *TGO+DX 
YF= ( ( AY*TGO+BY ) *TGO+CY ) *TGO+DY 
ZF-( ( AZ*TGO+BZ ) *TGO+CZ ) *TGO+DZ 
XFDOT«= (3.0  *AX*TGO+J3X+BX ) *TGO+CX 
YFDOT*= (3.0  *AY*TGO+BY+BY ) *TGO+CY 
ZFDOT=( 3 . 0*AZ*TGO+BZ+BZ ) *TGO+CZ 
DTGO-TGO-H 

CALL  SPLINE ( XDUAL2 ( 1 )  , XDUAL2 ( 2 ) , XF , XFDOT , 

+  DTGO , AX , BX , CX , DX ) 

CALL  SPLINE ( XDUAL2 ( 3 ) , XDUAL2 ( 4 ) , YF, YFDOT , 

+  DTGO , AY , BY, CY, DY) 

CALL  SPLINE( XDUAL2 ( 5 ) , XDUAL2 ( 6 ) , ZF , ZFDOT , 

+  DTGO / AZ , BZ , CZ , DZ ) 

C  COMPUTE  NEW  CERTAINTY  EQUIVALENCE  SOLUTION 

CALL  SEARCHA ( AX , BX , CX , DX , AY , BY , CY , DY , 

+  AZ , BZ , CZ , DZ , K , DTGO , DVYA , DVZ A , TOL , CFLAG ) 

C  ENSURE  NEW  C.E.  SOLUTION  IS  WITHIN  THRUST  LIMITS 

RTGO=DTGO-H 

RADICL«RTGO*RTGO- ( DVYA+DVYA) *RTGO*H/MAXDV 
IF  ( RADICL  .LE.  0.0)  THEN 
DVYA-MAXDV* INT ( RTGO/H ) /2 . 0 
COST2- ( DVYA+DVYA ) *  *2 
ELSE 

COST2«( (RTGO-SQRT( RADICL) ) +MAXDV/H ) * *2 
END  IF 

RAD I CL-RTGO  *  RTGO- ( DVZ A+DVZ A ) *  RTGO*  H/MAXDV 
IF  (RADICL  .LE.  0.0)  THEN 
DVZA-MAXDV* I NT ( RTGO/H ) /2 . 0 
COST2-COST2+ ( DVZA+DVZA ) *  *2 
ELSE 

COST2*COST2+( ( RTGO-SQRT( RADICL ) )*MAXDV/H)**2 
END  IF 

C  COMPUTE  NEXT  ITERATION  COST 
XDUAL2 ( 4 ) -XDUAL2 ( 4 ) -DVYA 
XDUAL2 ( 6 ) -XDUAL2 ( 6 ) -DVZA 
XPD2 ( 4 ) «XPD2 ( 4 ) +DVYA 
XPD2 ( 6 ) *XPD2 ( 6 ) +DVZA 
DH-DTGO/COUNT 
DO  400  1-1, COUNT 

RANGE-SQRT( XDUAL2 ( 1 ) *XDUAL2 ( 1 ) + 

+  XDUAL2 ( 3 ) *XDUAL2 ( 3 ) +XDUAL2 ( 5 ) *XDUAL2 ( 5 ) ) 

R3 { 1 , 1 ) -SIGMAMI 1 ) * RANGE* SIGMAM( 1 ) * RANGE 
CALL  EKF8 ( XDUAL2 , XED2 , XPD2 , TD , DH , CVD2 ,Q,R3, 

+  0.0,0. 0,0.0, KFLAG , RES , 2 ) 

400  CONTINUE 

CVTOT-CVD2 (1,1) +CVD2 (3,3) +CVD2 (5,5) 
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COST2=COST2+K* ( CVT0T+XDUAL2 ( 1 ) *XDUAL2 ( 1 ) + 

+  XDUAL2 ( 3 ) *XDUAL2 ( 3 ) +XDUAL2 ( 5 ) *XDUAL2 ( 5 ) ) + 
+  DVY  *  DVY + D VZ  *  DVZ 

C  CHOOSE  THE  LEAST  COST  OPTION 
IF  ( COSTl  .GT.  COST2)  THEN 

PRINT  *,'  DEVIATING  FROM  NOMINAL  PATH' 
DELTAY=DVY 
DELTAZ-DVZ 
TGO-DTGO+H 
ENDIF 

END 


SUBROUTINE  SEARCHT ( XP , XE , A , MDOT , H , T , TGO , 

+  DELTAY , DELTAZ , TOL , CFLAG , COUNT ) 

C  THIS  SUBROUTINE  VARIES  THE  TIME-TO-GO  (TGO)  AND  THE 
C  VELOCITY  CHANGES  (DELTAY  AND  DELTAZ)  WHILE  BRINGING 
C  MISS  DISTANCE  WITHIN  A  PRESPECIFIED  TOLERANCE. 

C  THIS  IS  ACCOMPLISHED  BY  EMPLOYING  A  NEWTON-RAPHSON 
C  SEARCH  SCHEME  FOR  NON-LINEAR  SYSTEMS.  ( PP .  176-179 
C  OF  MARON,  'NUMERICAL  ANALYSIS,  A  PRACTICAL  APPROACH') 
C  XF, YF,ZF , XFDOT, YFDOT,  AND  ZFDOT  ARE  COMPUTED 
C  NUMERICALLY. 

REAL* 8  XP ( 6 ) , XE ( 6 ) , XPD ( 6 ) , XED( 6 ), T , TGO , DELTAY 
REAL *8  XF , YF , Z F , SCALE , XFDOT , YFDOT , Z FDOT , TD , DTGO 
REAL* 8  A , MDOT , TOL , AD , MDOTD , OLDTGO , DH , DELTAZ , H 
REAL *8  DDY,DDZ 
INTEGER  I, J, COUNT, CFLAG 

C  INITIALIZE  VELOCITY  CHANGES 
DELTAY-0.0 
DELTAZ -0 . 0 
OLDTGO-TGO 

C  BEGIN  SEARCH  LOOP 
DO  10  J-1,10 

C  INITIALIZE  DUMMY  VARIABLES 
DO  110  1-1,6 
XED( I )-XE( I ) 

XPD(T)-XP(I) 

110  CONTINUE 

XPD( 4 )-XPD( 4 ) +DELTAY 
XPD( 6 ) =XPD( 6 ) +DELTAZ 
AD-A 

MDOTD-MDOT 


M  Ml 
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C  PROPAGATE  DUMMY  VARIABLES  FORWARD  TO  IMPACT  TIME 
TD-T 

DH=TGO/COUNT 
DO  115  1=1, COUNT 

CALL  RK4SYSP ( TD , XPD , DH ) 

CALL  RK4SYSE ( TD , XED , DH , AD , MDOTD ) 

TD=TD+H 

115  CONTINUE 

C  ASSIGN  FINAL  RELATIVE  POSITIONS 
XF=XED{ 1 ) -XPD( 1 ) 

YF=XED ( 3 ) -XPD ( 3 ) 

ZF=XED( 5 ) -XPD( 5 ) 
MISS=SQRT(XF*XF+YF*YF+ZF*ZF) 

C  TEST  FOR  NEARNESS 

IF  (MISS  .LE.  TOL)  RETURN 

C  ASSIGN  FINAL  RELATIVE  VELOCITIES 
XFDOT=XED ( 2 ) -XPD ( 2 ) 

YFDOT=XED( 4 )-XPD( 4 ) 

ZFDOT=XED( 6 ) -XPD( 6 ) 

C  COMPUTE  CHANGES  IN  CONTROL  PARAMETERS  AND  UPDATE 
DTGO— XF/XFDOT 
DDY* ( DTGO* YFDOT+YF ) /TGO 
DDZ= ( DTGO* ZFDOT+Z F ) /TGO 
TGO-TGO+DTGO 
DELTAY-DELTAY+DDY 
DELTAZ-DELTAZ+DDZ 

C  TEST  FOR  CONVERGENCE  OF  TIME-TO-GO 

IF  (ABS(DTGO)  .LT.  ( . 000001*TGO ) )  RETURN 

10  CONTINUE 

C  INCREMENT  CONVERGENCE  FLAG 
CFLAG-CFLAG+1 

C  ZERO  OUT  VELOCITY  CHANGES  AND  RESET  TIME 
DELTAY-0.0 
DELTAZ-0.0 
TGO-OLDTGO 


END 


APPENDIX  E 


IN- PLANE  THRUST  PROFILES 

This  appendix  contains  the  in- plane  thrust  profiles  for 
Cases  I  and  V,  showing  the  effect  of  estimate  uncertainty  on  the 
various  control  strategies.  Each  profile  was  started  with  the 
same  random  seed  to  form  a  basis  for  comparison. 
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Figure  E-2.  la- plane  thrust  profile  of  Plan  B  for  Case  I. 
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Figure  E-3.  In- plane  thrust  profile  of  Plan  C  for  Case  I. 


0.0 


10.0 


30. 


Figure  E-4. 
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■plane  Optiaiuo  Thrust  Spacing  profile  for  Case 
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Figure  E-6.  In- plane  thrust  profile  of  Certainty  Control 
for  Case  I. 
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Figure  E- 13.  In- plane  thrust  profile  of  Certainty  Control 
for  Case  V. 
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is  constrained  to  lateral  thrusting  with  the  evader  modeled  as  an 
ICBM  in  its  final  boost  phase.  Proportional  navigation,  optimal 
control  using  certainty  equivalence,  dual  control,  and  control 
with  optimum  thrust  spacing  are  all  examined.  Also,  a  new 
approach  called  certainty  control  is  developed  for  this  problem. 
This  algorithm  constrains  the  final  state  to  a  function  of 
projected  estimate  error  to  reduce  control  energy  expenditure. 
All  methods  model  the  trajectories  using  splines  and  employ  eight 
state  Extended  Kalman  Filters  with  line- of- sight  and  range 
updates.  The  relative  effectiveness  of  these  control  strategies 
is  illustrated  by  applying  them  to  various  intercept  problems. 


